» 


NAVAL  POSTGRADUATE  SCHOOL 

Monterey,  California 


ft  ilSlii 

THESIS 


A  THREE  DIMENSIONAL 

NON-SINGULAR  MODELLING  OF 

RIGID 

MANIPULATORS 

by 

Sadrettin  Altinok 

- 

December  1987 

The 

sis 

Advisor 

D.  L.  Smith 

Approved  for  public  release;  distribution  is  unlimited. 


T238672 


SECuR'Tv  CLASSIFICATION  OF  Tii'S  PAGE 


REPORT  DOCUMENTATION  PAGE 


la    REPORT  SECURITY  CLASSIFICATION 

UNCLASSIFIED 


10    RESTRICTIVE  MARKINGS 


2a    SECURITY  C^ASSiF.CATiON  AUTHORITY 


20    DECLASSIFICATION/ DOWNGRADING  SCHEDULE 


3     DISTRIBUTION,  AVAILABILITY  OF  REPORT 

APPROVED  FOR  PUBLIC  RELEASE 
DISTRIBUTION  IS  UNLIMITED 


4    PERFORMING  ORGANIZATION  REPORT  NUMBER(S) 


5    MONITORING  ORGANIZATION  REPORT  NUMSER(S) 


6a    NAME  OF  PERFORMING  ORGANIZATION 

NAVAL  POSTGRADUATE  SCHOOL 


6o    OFFICE  SYMBOL 
(If  applicable) 

69 


7a    NAME  OF  MONITORING  ORGANIZATION 

NAVAL  POSTGRADUATE  SCHOOL 


6c   ADDRESS  iC/fy.  Srare.  and  Zip  Code) 

MONTEREY,  CALIFORNIA  93943-5000 


70    ADDRESS  (City.  State,  and  ZIP  Code) 

MONTEREY,  CALIFORNIA  93943-5100 


8a    NAME  0=  FUNDING- SPONSORING 
ORGANIZATION 


8b   OFFICE  SYMBOL 
(If  applicable) 


9    PROCUREMENT  INSTRUMENT  IDENTIFICATION  NUMBER 


8c   ADDRESS  (City,  State,  ana  ZIP  Code) 


10    SOURCE  OF  FUNDING   NUMBERS 


PROGRAM 
ELEMENT  NO 


PROJECT 
NO 


TASK 
NO 


WORK   UNIT 
ACCESSION  NO 


1 1    TITLE  (include  Security  Classification) 

A  THREE  DIMENSIONAL  NON-SINGULAR  MODELLING  OF  RIGID  MANIPULATORS 


12  PERSONAL  AUTHOR(S) 


SADRETTIN  ALTINOK 


13a    TYPE  OF  REPORT 

MASTERS  THESIS 


130    TIME  COVERED 
FROM TO 


14    DATE  OF  REPORT   (Year.  Month,  Day) 

1987  DECEMBER  


15    PAGE  COUNT 
1C6 


16    SUPPLEMENTARY  NOTAT.ON 


COSATi  CODES 


FIELD 


GROUP 


SUB-GROUP 


18    SUBJECT  TERMS  {Continue  on  reverie  if  necessary  and  identify  by  block  number) 

ROBOTICS,  ROBOT  SIMULATION,  SINGULARITY,  MODELLING 
DYNAMIC  EQUATIONS  OF  MOTION,  NON-SINGULAR 


19   ABSTRACT  (Continue  on  reverse  if  necessary  and  identify  by  block  number) 

There  are  several  common  approaches  used  to  obtain  the  kinematic  and 
dynamic  equations  which  describe  the  motion  of  robot  manipulators. 
However,  a  problem  arises  when  these  conventional  body  oriented  robot 
arm  kinematic  equations  are  used  to  simulate  the  manipulator  motion.  In 
this  case,  the  jacobian  matrix  which  relates  the  end  effector  motion  to 
joint  angle  variations  becomes  singular  when  two  succesive  arm  links 
align.  When  the  robot  arm  passes  through  these  singular  points,  the 
jacobian  matrix  is  not  invertible,  and  a  result  of  this,  the  motion 
cannot  be  simulated.  This  thesis  investigates  how  this  situation  can  be 
avoided  by  using  a  Newton  Euler  approach  to  variable  definition,  and 
using  the  equations  interpretted  in  a  fixed  reference  frame. 


20    DISTRIBUTION/ AVAILABILITY  OF  ABSTRACT 
S3  UNCLASSIFIED/UNLIMITED       □  SAME  AS  RPT 


□  OTIC  USERS 


21    ABSTRACT  SECURITY  CLASSIFICATION 

UNCLASSIFIED 


22a    NAME  OF  RESPONSIBLE  INDIVIDUAL 

DAVID  SMITH 


22b  TELEPHONE  (Include  Area  Code) 

(408)646-3383 


^jiC    Of 

69Sm 


OFFICE  SYMBOL 


DDFORM  1473,  84  mar 


13  APR  edition  may  be  used  until  exnausteo 
ah  otner  editions  are  obsolete 


SECURITY  CLASSIFICATION  QF  THIS  PAGE 

O  U.S    Oo.i'««mi  Pttntlflt  Ol'ict     Illt-lOHt. 

UNCLASSIFIED 


Approved  for  public  release;  distribution  is  unlimited. 

A  Three  Dimensional 
Non-Singular  Modelling  of  Rigid  Manipulators 


by 


Sadrettin  Altinok 

1st.  Lieutenant,  Turkish  Army 

B.S.,  Turkish  Army  Academy,  1981 


Submitted  in  partial  fulfillment  of  the 
requirements  for  the  degree  of 


MASTER  OF  SCIENCE  IN  MECHANICAL  ENGINEERING 


from  the 


NAVAL  POSTGRADUATE  SCHOOL 

December  1987 


ABSTRACT 
There  are  several  common  approaches  used  to  obtain  the 
kinematic  and  dynamic  equations  which  describe  the  motion  of 
robot  manipulators.  However,  a  problem  arises  when  these 
conventional  body  oriented  robot  arm  kinematic  equations  are 
used  to  simulate  the  manipulator  motion.  In  this  Case,  the 
jocobian  matrix  which  relates  the  end  effector"  motion  to 
joint  angle  variations  becomes  singular  when  two  successive 
arm  links  align.  When  the  robot  arm  passes  through  these 
singular  points,  the  jacobian  matrix  is  not  invertible,  and 
a  result  of  this,  the  motion  cannot  be  simulated.  This 
thesis  investigates  how  this  situation  can  be  avoided  by 
using  a  Newton  Euler  approach  to  variable  difinition,  and 
using  the  equations  interpretted  in  a  fixed  reference  frame. 


TABLE  OF  CONTENTS 


I .  INTRODUCTION 25 

II .  THEORETICAL  DEVELOPMENT 28 

A.  THEORY  OF  THE  SOLUTION  28 

B.  DYNAMIC  EQUATIONS  OF  MOTION  FOR  LINK  ONE  .......  30 

1 .  Force  Equations  ." 30 

2 .  Joint  Equations  31 

3 .  Moment  Equations  32 

C.  DYNAMIC  EQUATIONS  OF  MOTION  FOR  LINK  TWO  36 

1 .  Force  Equations  36 

2 .  Joint  Equations  37 

3 .  Moment  Equations 39 

D.  DYNAMIC  EQUATIONS  OF  MOTION  FOR  LINK  THREE  41 

1 .  Force  Equations  41 

2 .  Joint  Equations 41 

3 .  Moment  Equations 43 

III .  COMPUTATIONAL  APPROACH 45 

A.  PROGRAM  MATRICIES  45 

,B.  CONSTRAINTS  IN  THE  SIMULATION  PROGRAM  50 

C.  PROGRAM  VALIDATION  52 

IV.  RESULTS  AND  RECOMMENDATIONS  61 

APPENDIX  A:  DERIVATION  OF  THE  TRANSFORMATION 

MATRIX  63 

APPENDIX  B:  DIRECT  DYNAMICS  SIMULATION  PROGRAM  1  67 

4 


DUD 

5002 


APPENDIX  C:  DIRECT  DYNAMICS  SIMULATION  PROGRAM  2  83 

LIST  OF  REFERENCES  ■ .101 

INITIAL  DISTRIBUTION  LIST  ■ 104 


LIST  OF  FIGURES 

1 .  Free  Body  Diagram  of  a  Three  Link  Manipulator  ......  29 

2 .  Computer  Simulation  Flow  Chart 46 

3 .  Matrix  Entries 47 

4 .  Moment  of  Inertia  51 

5.  Validation  Procedure  Flow  Chart 54 

6.  Initial  Orientation  of  Links  for  Validation  ........  55 

7 .  Configuration  A,  Link  2  Wx  Motion  57 

8.  Configuration  A,  Link  2  Wz  Motion  58 

9.  Percent  Error  Between  Theoretical  and 

Simulated  Angles  59 

10 .  Critical  Angles 66 


TABLE  OF  THE  SYMBOLS  AND  ABBREVIATIONS 


COMPUTER 

SYMBOL 

A 

AA 

AB 
AG1 


AG  2 


AG  3 


AOX 


AOY 


AOZ 


TEXT 

VARIABLE 

A 

Aa 
Ab 
Agl 


Ag2 


Ag3 


aox 


aoy 


aoz 


DESCRIPTION 

Sine  Wave  Input  Torque 

Amplitude 

Acceleration  of  Point  A 

Acceleration  of  Point  B 

The   Acceleration  Vector 

of  the   Center  of  Gravity 

for  Link  1 

Same  as   AG1  but  for  Link 

2 

Same  as  AG1  but   for  Link 

3 

Linear  Acceleration   of 

Joint  Zero  in  the  X 

Direction 

Linear  Acceleration   of 

Joint  Zero  in  the  Y 

Direction 

Linear  Acceleration   of 

Joint  Zero  in  the  Z 

Direction 


AX1  axl  Linear  Acceleration  of 

Link  One  in  the  X 

Direction 
AY1  ayl  Linear  Acceleration  of 

Link  One  in  the  Y 

Direction 
AZ1  '  azl  Linear  Acceleration  of 

Link  One  in  the  Z 

Direction 
AX2  ax2  Linear  Acceleration  of 

Link  Two  in  the  X 

Direction 
AY2  ay2  Linear  Acceleration  of 

Link  Two  in  the  Y 

Direction 
AZ2  az2  Linear  Acceleration  of 

Link  Two  in  the  Z 

Direction 
AX3  ax3  Linear  Acceleration  of 

Link  Three  in  the  X 

Direction 
AY3  ay3  Linear  Acceleration   of 

Link  Three  in  the  Y 

Direction 


AZ3 


az3 


BRATEK3) 


BRATE2(3) 

BRATE3(3) 

DEGRA 

DRCANX(l) 
DRCANX(2) 
DRCANX ( 3 ) 


DRCANY(l) 
DRCANY ( 2 ) 
DRCANY(3) 


DRCANZ(l) 
DRCANZ(2) 


Bratel 


Brate2 


Brate3 


Linear  Acceleration  of 

Link  Three  in  the  Z 

Direction 

Angular   Velocity  Vector 

in  Body   Fixed  (rotating) 

Coordinate  System   for 

Link  1  in  the  x,  y   and  z 

Direction  Respectively 

Same  as   Bratel  but  for 

the  Link  2 

Same  as  Bratel  but   for 

the  Link  3 

Conversion   from  Degrees 

to  Radians 

Direction  Cosine   Angle 

in  Degrees   in  Fixed 

Coordinate  System   from  X 

Axis  for   Link  1-3 

Respectively 

Direction  Cosine   Angle 

in  Degrees   in  Fixed 

Coordinate  System   from  Y 

Axis  for   Link  1-3 

Respectively 

Direction  Cosine  Angle 

in  Degrees  in  Fixed 


DRCANZ(3) 


DRCRAX(l) 
DRCRAX(2) 
DRCRAX(3) 


DRCRAY(l) 
DRCRAY(2) 
DRCRAY(3) 


DRCRAZ ( 1 ) 
DRCRAZ ( 2 ) 
DRCRAZ ( 3 ) 


DRCSX(l) 
DRCSX(2) 
DRCSX(3) 

DRCSY(l) 
DRCSY(2) 
DRCSY(3) 


Coordinate   System  from  Z 

Axis  for  Link  1-3 

Respectively 

Direction  Cosine   Angle 

in  Radians  in  Fixed 

Coordinate  System   from 

X  Axis  for  Link  1-3 

Respectively 

Direction  Cosine  Angle 

in  Radians   in  Fixed 

Coordinate  System   from 

Y  Axis   for  Link  1-3 

Respectively 

Direction  Cosine  Angle 

in  Radians  in  Fixed 

Coordinate  System  from 

Z  Axis  for  Link  1-3 

Respectively 

The  Argument  of  Direction 

Cosine  Angle  in  the  x 

Direction  for  Link  1-3 

Respectively 

The  Argument  of  Direction 

Cosine  Angle   in  the  y 

Direction  for  Link  1-3 

Respectively 


10 


DRCSZ(l) 
DRCSZ(2) 
DRCSZ(3) 

DQ(27) 


FXO 


FYO 


FZO 


FX1 


FY1 


FZ1 


FX2 


FY2 


FZ2 


DQ 


Fxo 


Fyo 


Fzo 


Fxl 


Fyl 


Fzl 


Fx2 


Fy2 


Fz2 


S 


The  Argument  of  Direction 
Cosine  Angle  in  the  z 
Direction  for   Link  1-3 
Respectively 
A  27*1  Column  Matrix 
Obtained  by  Multiplying 
the  MatA  and  MatB 
Computed  Force  in  the   X 
Direction  at  Joint  0 
Computed  Force   in  the  Y 
Direction  at  Joint  0 
Computed  Force  in  the   Z 
Direction  at  Joint  0 
Computed  Force   in  the  X 
Direction  at  Joint  1 
Computed  Force  in  the   Y 
Direction  at  Joint  1 
Computed  Force   in  the  Z 
Direction  at  Joint  1 
Computed  Force  in  the   X 
Direction  at  Joint  2 
Computed  Force   in  the  Y 
Direction  at  Joint  2 
Computed  Force  in  the   Z 
Direction  at  Joint  2 
Gravitional  Constant 


11 


HDX(2)         HDx  The  Time  Rate  of  Change 

of  Angular  Momentum  of  a 
Two  Elements   Composite 
Body  in  the  X  Direction 

HDY(2)         HDy  Same  as  HDX  but   in  the  Y 

Direction 

HDZ(2)         HDz  Same  as   HDX  but  in  the  Z 

Direction 

I  Counter 

IA  Row  Dimension   of  Matrix 

A  and  Matrix  B  Used  in 
LEQ2TF  Subroutine 

IER  Error  Parameter   Used  in 

Subroutine  LEQT2F 

IDGT  Accuracy  Test   Used  in 

Subroutine  LEQT2F  for 
Iterative  Improvement 

IXX(3,2)       Ixx  A  3*2  Matrix  of  Moment 

of  Inertia  for  the  Two 
Element  Composite  Body 
of  Link  1-3  About  X  Axis 

IYY(3,2)        Iyy  Same  as  IXX  but  About 

the  Y  Axis 

IZZ(3,2)        Izz  Same  as   IXX  but  About 

the  Z  Axis 


12 


IXZ(3,2) 


Ixz 


IXY(3,2) 


IYZ(3,2) 


IXXT(3) 


IYYT(3) 


IZZT(3) 


IXZT(3) 


IXYT(3) 


IYZT(3) 


JXO 


JYO 


Ixy 


Iyz 


jxo 


J  70 


A  3*2   Matrix  of  Products 

of  Inertia  for  the  Two 

Element  Composite   Body 

of  Link   1-3  About  the  XZ 

Coordinate  Axis 

Same  as  IXZ  but   for  the 

XY  Axis 

Same  as   IXZ  but  for  the 

YZ  Axis 

Total  Moment   of  Inertia 

of  Link  1-3  About  X  Axis 

Same  as   IXXT  but  About 

the  Y  Axis 

Same  as  IXXT  but   About 

the  Z  Axis 

Same  as   IXXT  but  About 

the  XZ  Axis 

Same  as  IXXT  but   About 

the  XY  Axis 

Same  as   IXXT  but  About 

the  YZ  Axis 

Location  of  Joint  Zero 

in  the  X  Direction 

Location  of   Joint  Zero 

in  the  Y  Direction 


13 


JZO 


JX1 


JY1 


JZ1 


JX2 


JY2 


JZ2 


L(3,2) 


LCOGX(3) 


LCOGY(3) 


LCOGZ(3) 


JZO 


jxl 


jyl 


jzl 


jx2 


J72 


jz2 


L(3,2) 


LCOGx 


LCOGy 


LCOGz 


Location  of  Joint  Zero 

in  the  Z  Direction 

Location  of   Joint  One  in 

the  X  Direction 

Location  of   Joint  One  in 

the  Y  Direction 

Location  of   Joint  One  in 

the  Z  Direction 

Location  of   Joint  Two  in 

the  X  Direction 

Location  of   Joint  Two  in 

the  Y  Direction 

Location  of   Joint  Two  in 

the  Z  Direction 

A  3*2   Matrix  that  is  the 

Distance  from   Center  of 

Link  to  Center  of  Mass  at 

Each  Link  End 

A  1*3   Location  of  Center 

of  Gravity   Vector  for 

Link  1-3  in  the  X 

Direction 

Same  as   LCOGX  but  for 

the  Y  Direction 

Same  as  LCOGX  but  for 

the  Z  Direction 


14 


M 


MASS (3,2) 


MATB(27) 


MATC(27) 


MAT1R, 
MAT2R, 


Mass(3, 2) 


MASS1 

Ml 

MASS2 

M2 

MASS3 

M3 

MATA(27,27) 

MatA 

MatB 


MatC 


Matlr 
Mat2r 


Number  of   the  Right  Hand 
Side  Used  in  LEQT2F 
A  3*2  Matrix  of   Mass  of 
Each  Element   that  Make 
Up  the  Composite  Body 
for  Link  1-3 
Total  Mass  of  Link  1 
Total  Mass  of  Link  2 
Total  Mass  of  Link  3 
A  27*27  Matrix  Consisting 
of  Coefficients  of  the 
Unknown  Variables 
A   27*1  Vector  Consisting 
of  the  Coefficient  of 
Known  Variables  on  Input 
and  an  Output  the  Solut- 
ion to   the  Linear  Equat- 
ions 

A  27*1  Column  Matrix 
which  Contains   the 
Elements  of  the  Known 
MatB     in    Simulation 
Process 

Transformation   Matricies 
from  Fixed   Coordinate 


15 


MAT3R 


Mat3r 


MAT IT, 

Mat  It 

MAT2T, 

Mat2t 

MAT3T 

Mat3t 

MI 


MJ 


MK 


MIAO.MJAO 
and  MKAO 


MIBO.MJBO 
and  MKBO 


System  to  Body  Fixed 

(Rotating)  Coordinate 

System  for  Link  1   Link  2 

and  Link  3  Respectively 

Transformation  Matricies 

from  Body  Fixed 

Coordinate  System  to  Yaw 

Pitch  and  Roll  Angles 

Coordinate  System  for 

Link  1   Link  2  and  Link  3 

Respectively 

Results  From  Subroutine 

CPROD,  I   Component  of 

Vector  Cross  Product 

J  Component  of  Vector 

Cross  Product 

K  Component   of  Vector 

Cross  Product 

Cross  Product  Between 

WD1  and   RB/G1  at  Joint 

Zero  Link  One,  in  X,  Y,  Z 

Direction 

Cross  Product   Between 

Wl  and  RB/G1  at  Joint 

Zero  Link  One,  in  X,  Y,  Z 

Direction 


16 


MICO,MJCO 
and  MKCO 


MIA1.MJA1 
and  MKA1 


MIB1.MJB1 
and  MKB1 


MIC1.MJC1 
and  MKC1 


MIA2,MJA2 
and  MKA2 


MIB2.MJB2 
and  MKB2 


Cross  Product  Between 

Wl  and  MIBO,  MJBO,  MKBO 

at  Joint  Zero  Link  One, 

in  X,  Y,  Z  Direction 

Cross  Product   Between 

WD1  "and  RA/G1  at   Joint 

One  Link  One,  in  X,  Y,  Z 

Direction 

Cross  Product   Between  Wl 

and  RA/G1   at  Joint  One 

Link  One,  in  X,  Y,  Z 

Direction 

Cross   Product  Between  Wl 

and  MIB1,  MJB1 ,  MKB1   at 

Joint  One  Link  One,  in  X, 

Y,  Z  Direction 

Cross  Product  Between 

WD2  and  RB/G2  at  Joint 

One  Link  Two,  in  X,  Y,  Z 

Direction 

Cross   Product  Between  W2 

and  RB/G1  at  Joint  One 

Link  Two,   in  X,  Y,  Z 

Direction 


17 


MIC2,MJC2 
and  MKC2 


MIA3,MJA3 
and  MKA3 


MIB3,MJB3 
and  MKB3 


MIC3,MJC3 
and  MKC3 


MIA4,MJA4 
and  MKA4 


MIB4.MJB4 
and  MKB4 


Cross  Product   Between  W2 

and  MIB2,   MJB2 ,  MKB2  at 

Joint  One  Link  Two,  in  X, 

Y,  Z  Direction 

Cross  Product   Between 

WD2  and  RA/G2  at   Joint 

Two  Link   Two,  in  X,  Y,  Z 

Direction 

Cross  Product   Between  W2 

and  RA/G2   at  Joint  Two 

Link  Two,  in  X,  Y,  Z 

Direction 

Cross   Product  Between  W2 

and  MIB3,  MJB3 ,  MKB3   at 

Joint  Two  Link  Two,  in  X, 

Y,  Z  Direction 

Cross  Product  Between 

WD3  and  RA/G3  at  Joint 

Two  Link  Three,  in  X,  Y, 

Z  Direction 

Cross   Product  Between  W3 

and  RA/G3  at  Joint   Two 

Link  Three,   in  X,  Y,  Z 

Direction 


18 


MIC4.MJC4 
and  MKC4 


N 

P 

PTRY(l) 

PTRY(2) 

PTRY(3) 

PTCANY(l) 

PTCANY(2) 

PTCANYO) 

RADEG 

RATEK3) 


RATE2(3) 


RATE3(3) 


Ratel 


Rate2 


Rate3 


Cross  Product   Between  W3 
and  MIB4,   MJB4 ,  MKB4  at 
Joint  Two   Link  Three,  in 
X,  Y,  Z  Direction 
Order  of   MATA  and  tt  of 
Rows  in  MATB 
Phase  Angle  of  Sine  Wave 
Pitch  Angle  in  Radians 
for  Link  1-3  Respectively 

Pitch  Angle  in  Degrees 
for  Link  1-3  Respectively 

Conversion  from  Radians 

to  Degrees 

Angular   Velocity  Vector 

in  Yaw  Pitch  and  Roll 

Coordinate  System   for 

Link  1  in  the  x,  y   and  z 

Direction  Repectively 

Same  as   Ratel  but  for 

the  Link  2 

Same  as  Ratel  but  for 

the  Link  3 


19 


RX(3,2) 


Rx(3,2) 


RY(3,2) 


RZ(3,2) 


RAG1 ( 3 ) 


RBG1 ( 3 ) 


RAG2 ( 3 ) 


RBG2 ( 3 ) 


RY(3,2) 


Rz(3,2) 


ra/Gl 


rb/Gl 


ra/G2 


rb/G2 


A  3*2  Matrix  Consisting 
of  the   Distance  from  the 
COG  of  the  Link  to  Center 
of  for  Elements  of   Link 
1-3  in  the  X  Direction 
Same  as   RX(3,2)  but  in 
the  Y  Direction 
Same  as  RX(3,2)  but   in 
the  Z  Direction 
A  1*3   Vector,  Distance 
of  Point  A  to  COG  for 
Link  One,   in  X,  Y,  Z 
Direction 

A  1*3  Vector,  Distance 
of  Point  B  to  COG  for 
Link  One,  X,  Y,  Z  Direct- 
ion 

A  1*3  Vector,  Distance  of 
Point  A  to  COG   for  Link 
Two,  in  X,  Y,  Z  Direction 
A  1*3  Vector,  Distance  of 
Point  B  to  COG   for  Link 
Two,  in  X,  Y,  Z  Direction 


20 


RBG3(3) 


rb/G3 


RLRZ ( 1 ) 
RLRZ ( 2 ) 
RLRZ ( 3 ) 


A  1*3  Vector,  Distance  of 

Point  B  to  COG  for  Link 

Three,  in  X,  Y,  Z 

Direction 

Roll  Angle  in  Radians  for 

Link  1-3  Respectively 


ROLANZ(l) 
R0LANZ(2) 
R0LANZ(3) 
SUMHDX(3) 


SUMHDY(3) 


SUMHDZ(3) 


HDx 


HDy 


HDz 


TOX,TOY, 

Tox, 

Toy 

TOZ 

Toz 

T1X.T1Y 

Tlx, 

Tly 

T1Z 

Tlz 

T2X,T2Y 

T2x, 

T2y 

T2Z 

T2z 

TIPX.TIPY 

Roll  Angle  in  Degrees  for 
Link  1-3  Respectively 

A  1*3   Vector,  Sum  of  the 

HDX  for  the  Two  Elements 

of  Link   1-3  in  the  X 

Direction 

Same  as  HDX  but   in  the  Y 

Direction 

Same  as   HDX  but  in  the  Z 

Direction 

Input  Torque  at  Joint   0 

in  X,  Y,  Z  Direction 

Input  Torque  at  Joint  One 

in  X,  Y,  Z  Direction 

Input  Torque  at  Joint  Two 

in  X,  Y,  Z  Direction 

Position  of   the  End 


21 


TIPZ 

VECTAO ( 3 ) 
VECTBO ( 3 ) 

VECTA1 ( 3 ) 
VECTBK3) 
VECTA2(3) 

VECTB2  0) 

VECTA(3) 

VECTBO) 

W 

WG1,WG2 

WG3 

Wl(3) 


W2(3) 


W3(3) 


WDX(3) 


w 

wgl,wg2 
wg3 
wl(3) 


w2(3) 


w3(3) 


wdx(3) 


Effector 

1*3  Vector  Used  in  Subro- 
utine CPROD   for  Joint 
Zero 

1*3  Vector  Used  in  Subro- 
utine CPROD  for  Joint  One 
1*3  Vector  Used  in  Subro- 
utine CPROD  for  Joint  Two 
1*3  Vector  Used  in  Subro- 
utine CPROD 

Frequency  of  Sine  Input 
Weight  of   Links  1,  2,  3 

A  1*3   Vector  of  the 

Angular  Velocity  of  Link 

1    in   x,    y,    and   z 

Direction  Respectively 

Same  as  Wl(3)  but  for 

the  Link  2 

Same  as   Wl(3)  but  for 

the  Link  3 

Angular  Acceleration   of 

Link  1-3   in  the  X 

Direction 
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WDY(3) 


wdy(3) 


WDZ(3) 


WKAREA 
X1,X2,X3 


Y1.Y2.Y3 


YWXR(l) 

YWXR(2) 

YWXR(3) 

YAWANX(l) 

YAWANX(2) 

YAWANX(3) 

Z1.Z2.Z3 


wdz(3) 


Angular  Acceleration   of 

Link  1-3   in  the  Y 

Direction 

Angular  Acceleration   of 

Link  1-3   in  the  Z 

Direction 

Work  Area   for  the  LEQT2F 

Location  of   the  COG  for 

Link  1-3  in  the  X 

Direction 

Location  of   the  COG  for 

Link  1-3  in  the  Y 

Direction 

Yaw  Angle   in  Radians  for 

Link  1-3  Respectively 

Yaw  Angle   in  Degrees  for 
Link  1-3  Respectively 

Location  of  the  COG   for 
Link  1-3   in  the  Z 
Direction 
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I.  INTRODUCTION 

The  study  of  robotics  is  a  fairly  new  discipline. 
Although  the  roots  of  these  studies  and  developments  can  be 
traced  back  to  the  1940' s,  the  first  commercial  computer 
controlled  robot  was  not  introduced  until  the  late  1950' s 
[Ref.  1].  Furthermore,  as  the  theory  developed,  several 
common  problemmatical  methods  have  been  widely  accepted  and 
used. 

When  robot  motion  is  studied,  it  is  usually  divided 
into  two  parts:  robot  arm  dynamics  and  robot  arm  kinematics. 
While  the  kinematics  problem  deals  with  the  geometry  of  the 
arm  links,  the  dynamics  problem  deals  with  the  study  of 
forced  motion.  The  dynamics  problem  is  further  divided  into 
two  parts:  the  direct  dynamics  problem  and  the  inverse 
dynamics  problem.  In  the  inverse  dynamics  problem,  link 
variables  such  as  acceleration  and  velocity  are  known  and 
the  forces  and  necessary  joint  torques  for  the  desired 
motion  are  calculated.  In  the  direct  dynamics  problem,  the 
joint  torques  are  known  and  the  accelerations  and  velocities 
of  each  joint  are  calculated. 

The  kinematics  problem  is  also  divided  into  two  parts: 
the  direct  kinematics  problem  and  the  inverse  kinematics 
problem.  The  direct  kinematics  problem  is,  given  a  set  of 
critical  geometric   joint  and  link  variables  for  each  of  the 
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joint-link  pairs  and  the  joint  angle  vector,  determine  the 
position  and  orientation  of  the  end  effector  of  the 
manipulator.  The  Denavit-Hartenberg  representation,  which 
uses  a  homogeneous  transformation  matrix  to  describe  the 
spatial  relationships  between  two  adjacent  rigid  mechanical 
links  is  the  most  common  method  used  to  study  the  direct 
kinematics  problem  [Ref.  2].  The  inverse  kinematics  problem 
is,  given  a  desired  position  and  orientation  of  the  end 
effector  of  the  manipulator  and  a  set  of  critical  geometric 
joint  and  link  parameters,  determine  the  corresponding  joint 
angle  vector;  i.e.,  find  all  of  the  joint  angles  of  the 
robot  arm  so  that  the  end  effector  can  be  positioned  in  the 
desired  location. 

A  difficulty  in  the  solution  to  the  inverse  kinematics 
problem  arises  when  two  successive  links  align  [Ref.  3].  At 
these  times  the  angle  between  two  successive  links  becomes  0 
or  180  degrees,  and  the  Jacobian  matrix  which  relates  the 
end  effector  motion  to  the  joint  variable  variations  cannot 
be  inverted.  This  means  that  motion  cannot  be  simulated. 
Different  approaches  to  this  problem  have  been  investigated. 
One  method  deals  with  the  Newton-Euler  approach  with  a 
moving  coordinate  system  [Refs.  4,  5],  another  uses  the 
Langrangian  approach  [Refs.  6,  7].  One  method  deals  with  a 
virtual  work  approach  [Ref.  8].  Kane's  dynamics  equations 
have   been   used   due   to  computational  efficiency  [Ref.  9]. 
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However,  none  of  these  methods  have   been  able   to  overcome 
this  singularity  problem  [Ref.  3]. 

Several  methods  have  been  proposed  to  avoid  the 
singular  configuration.  One  method  proposed  to  minimize  the 
time  near  the  singular  points  [Ref.  10],  thereby  reducing 
their  effects.  In  another  method,  it  was  proposed  to  avoid 
these  singular  points  by  confining  the  motion  [Ref.  11]. 
Other  solutions  deal  with  presenting  equations  that  can 
translate  the  manipulator  in  the  neighborhood  of  a 
singularity  through  identification  of  singular  points 
beforehand  [Refs.  12,  13,  14].  It  has  also  been  shown  that 
the  redundancy  of  robot  manipulators  is  effective  in  dealing 
with  the  singularities  [Refs.  15,  16,  17]. 

In  this  thesis  the  equations  of  motion  are  derived 
using  the  principles  of  Newtonian  dynamics  in  terms  of  a 
globally  fixed  coordinate  system  to  overcome  the  singularity 
problem.  Each  link  is  treated  as  a  free  body  with  forces 
and  moments  applied  at  the  joints  and  free  body  analysis  is 
used  to  derive  the  equations  of  motion.  Although  the 
equations  are  relatively  long  and  the  solution  to  the 
problem  is  computationally  time  consuming,  it  is  shown  that 
these  equations  do  overcome  the  singularity  problem.  The 
direct  dynamics  and  the  inverse  dynamics  problem  are  both 
simulated. 
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II.  THEORETICAL  DEVELOPMENT 

A.    THEORY  OF  THE  SOLUTION 

To  derive  the  non-singular  equations  of  motion  the 
Newton-Euler  approach  is  used  (Figure  1).  Each  link  is 
treated  as  a  free  body  with  forces  and  moments  applied  to 
it,  weight  has  been  disregarded.  The  globally  fixed  X  Y  Z 
coordinate  system  is  used  for  the  equations.  All  links  are 
assumed  to  be  rigid,  so  the  effects  of  flexibility  are  not 
considered.  All  of  the  distances  and  the  directions  of  the 
forces  and  moments  have  been  based  on  the  fixed  coordinate 
system  rather  than  a  local  coordinate  system  which  moves 
with  the  link  [Refs.  4,  5].  The  link  masses,  the  initial 
link  positions  and  the  orientations  are  assumed  to  be  known 
parameters.  As  a  result  of  equation  derivation  in  the  fixed 
reference  frame,  the  moment  of  inertia  is  allowed  to  change 
with  respect  to  time  and  is  calculated  for  each  small 
integration  interval.  This  is  opposed  to  keeping  inertia 
constant  as  used  in  the  local  coordinate  formulations.  But 
it  is  assumed  that  the  moment  of  inertia  is  constant  in  each 
small  integration  interval.  This  last  assumption 
effectively  linearizes  the  equations  of  motion  so  that  a 
non-singular  matrix  inversion  can  be  used  to  solve  the 
equation  set. 
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Figure  1 .  Free  Body  Diagram  of  a  Three  Link  Manipulator 
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To  calculate  the  moment  of  inertia  in  each  integration 
interval,  the  link  direction  cosine  angles  with  respect  to 
the  fixed  coordinate  system  were  used.  Acceleration  of 
joint  zero  was  input  as  zero.  For  each  link  the  three 
linear  acceleration  components,  three  angular  acceleration 
components  and  forces  at  each  joint  were  considered  to  be 
the  unknown  variables.  Based  on  the  Newtonian  dynamics  and 
the  manipulator  kinematics  [Ref.  18],  the  equations  were 
derived  as  follows: 

B.    DYNAMIC  EQUATIONS  OF  MOTION  OF  LINK  ONE 
1 .  Sum  of  Forces  Equations 

In  the  free  body  analysis  of  link  one  (Figure  1)  the 
sum  of  the  forces  in  the  x  direction  is: 

ZFx  =  Fxl  -  FxO  =  Mlaxl  (1) 

Similarly  sum  of  the  forces  in  the  y  direction  is: 

ZFy  =  Fyl  -  FyO  =  Mlayl  (2) 

and  the  sum  of  the  forces  in  the  z  direction  is: 

ZFz  =  Fzl  -  FzO  -  Wl  =  Mlazl  (3) 
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2 .  Joint  Equations 

We  begin  by  evaluating  the  joint  equations  at  joint 
zero  [Ref.  19,  equation  (8/4),  pp.  423].  If  the  joint  is 
sequested  and  analysis  conducted  at  a  point  on  link  zero 
(subscript  a)  and  another  at  a  point  on  link  one  (subscript 
b)  that  is  common  to  both,  so  when  linked  together  they  are 
equal.  This  results  in  two  equations  and  the  two  unknowns 
wdl  and  wl .   As  a  result: 

Aa  =  Ao 

which  is  the  acceleration  at  joint  zero,  and 

Ab  =  Al  +  (wdl  x  rb/Gl)  +  wl  x  (wl  x  rb/Gl ) 

which  is  the  acceleration  of  point  b  on  joint  one.  Here 
rb/Gl  is  the  distance  from  point  b  to  the  center  of  gravity 
of  link  one,  and  Al  is  the  acceleration  at  the  center  of 
mass  of  link  one  or, 

rb/Gl  =  ( jxO-LCOGxl)i  +  ( jyO-LCOGyl ) j  +  ( jzO-LCOGzl )k 
=  rb/Glx  +  rb/Gly  +  rb/Glz 

After  equating  Aa  and  Ab  and  having  the  known  variables  on 
the  right  side  of  the  equation  and  unknown  variables  on  the 
left  side  the  following  sets  of  equations  result: 
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Axl  +  wdyl(rb/Glz)  -  wdzl(rb/Gly)  =  Aox  -  MICO     (4) 

where  MICO  equals 

=  wylwxl(rb/Gly)  -  (wyl )2( rb/Glx)  -  (wzl )2(rb/Glx) 
+  wzlwxl(rb/Glz) 
also 

Ayl  +  wdzl( rb/Glx)  -  wdxl(rb/Glz)  =  Aoy  -  MJCO     (5) 

where  MJCO  equals 

=  wzlwyl(rb/Glz)  -  (wzl )2(rb/Gly)  -  (wxl )2(rb/Gly ) 
+  wxlwyl( rb/Glx) 
and 

Azl  +  wdxl(rb/Gly)  -  wdyl( rb/Glx)  =  Aoz  -  MKCO     (6) 
MKCO  equals 

=  wxlwzl( rb/Glx)  -  (wzl )2( rb/Glz )  -  (wyl )2(rb/Glz) 
+  wylwzl (rb/Gly) 

3 .  Sum  of  Moment  Equations 

Computing  the   sum  of  the  moment  equations  about  the 
center  of  gravity  results  in: 
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SMI  =  (rO/Gl  x  FO)  +  (rl/Gl  x  Fl )  -  Tl  +  TO 

where  the  vector  rO/Gl   is  the   distance  from   joint  zero  to 
the  center   of  gravity   of  link  one  and  vector  rl/Gl  is  the 
distance  from   joint  one  to  the   center  of   gravity  of  link 
one,  in  the  x,  y  and  z  directions.   Such  that 
rO/Gl  -    rjO  -  rGl 


and 


rl/Gl  =  rjl  -  rGl 


so 


and 


rjO  -  rGl  =  (xjO  -  xGl)i  +  (yjO  -  yGl)j  +  (zjO  -  zGl)k 


rjl  -  rGl  =  (xjl  -  xGl)i  +  (yjl  -  yGl ) j  +  (zjl  -  zGl)k 
In  the   x,  y   and  z   directions  the   sum  of  moment  equations 
are: 

ZM1  in  x  direction  - 

(yjO/Gl)FzO  +  (zjO/Gl)FyO  +  (yjl/Gl)Fzl  -  (zjl/Gl)Fyl 
-  Tlx  +  TOx  (7a) 

ZM1  in  y  direction^ 

(zjO/Gl)FxO  +  (xjO/Gl)FzO  +  (zjl/Gl)Fxl  -  (xjl/Gl)Fzl 
-Tly  +  TOy  (8a) 


33 


2M1  in  z  direction= 

(xjO/Gl)FyO  +  (yjO/Gl)FxO  +  (xjl/Gl)Fyl  -  (yjl/Gl)Fxl 
-Tlz  +  TOz  (9a) 

From  [Ref.19,  equation  (57),  pp.  227]  the  sum  of  the  moments 
about  a  fixed  point  that  does  not  move  with  the  body  is 
equal  to  the  time  rate  of  change  of  angular  momentum  of  the 
system  (H)  about  the  fixed  point,  2M  =  H.  In  the  present 
study  we  have  let  each  link  be  a  composite  body  of  two 
elements.  The  angular  momentum  (H)  for  a  composite  body 
where  the  number  of  elements  of  the  body  is  two,  about  the 
center  of  gravity  of  each  link  is  Hi  =2  [Ri  x  (w  x 
Ri)]Mi,  where  Ri  is  the  distance  from  the  center  of  gravity 
of  each  link  to  the  appropriate  element  (1  or  2)  in  the  x,  y 
and  z  direction.   So: 

Hx   =   2        [Ryi(wx(Ryi)   -  wy(Rxi))  -  Rzi(wz(Rxi)- 

wx(Rzi))]Mi 
Hx   =   [R2yl(wx)   -    Ryl(Rxl)(wy)   -    Rzi(Rxl)(wz)   + 

R2zl(wx)]Ml  +  [R2y2(wx)  -        Ry2(Rx2 ) (wy ) - 

Rz2(Rx2)(wz)    +    (R2z2)wx]M2 

If  Ixx  =   Ry2  +  Rz2  dm, 
and  Ixy  =   RxRy  dm, 
and  Ixz  =   RxRz  dm, 
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then: 

Hx  =  [Ilxx(wx)  -  Ilxy(wy)  -  Ilxz(wz)]Ml 

+  [I2xx(wx)  -  I2xy(wy)  -  I2xz(wz)]M2 
and 

HDx  =  [Ilxx(wdx)  -  Ilxy(wdy)  -  Ilxz(wdz)]Ml 

+  [I2xx(wdx)  -  I2xy(wdy)  -  I2xz(wdz)]M2      (7b) 

by  assuming  the  moment  of  inertia   changes  with  time  but  is 
constant  for  a  given  time  interval. 

By  similar  analysis  it  can  be  shown: 
Hy   =   2       [Rzi(wy(Rzi)   -   wz(Ryi))   -  Rxi(wx(Ryi)- 
wy(Rxi)  -  wy(Rxi))]Mi 
and  if  Iyy  =      Rx2  +  Rz2  dm, 
and    Iyz  =   RyRz  dm, 
and     Ixy  =      RxRy  dm, 
then: 

HDy  =  [Ilyy(wdy)  -  Ilyz(wdz)  -  Ilyz(wdx)]Ml 

+  [I2yy(wdy)  -  I2yz(wdz)  -  I2yx(wdx)]M2      (8b) 
and 

Hz  =  2      [   Rxi(wz(Rxi)   -   wx(Rzi))   -  Ryi(wy(Rzi)- 
wz(Ryi))]Mi 
if    Izz  =   Rx2  +  Ry2  dm, 
So    Hz  =  [Ilzz(wz)  -  Ilyz(wy)  -  Ilzx(wx)]Ml 

+  [I2zz(wz)  -  I2yz(wy)  -  I2zx(wx)]M2 
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then 

HDz  =  [Ilz'z(wdz)  -  Ilyz(wdy)  -  Ilzx(wdx)]Ml 

+  [I2zz(wdz)  -  I2yz(wdy)  -  I2zx(wdx)]M2      (9b) 

Combining  equations  (7a)  and  (7b)  and  keeping  known 
variables  on  the  right  side  and  unknown  variables  on  the 
left  side  yields: 

ZMlx  =  (-yjO/Gl)FzO  +  (zjO/Gl)FyO  +  (yjl/Gl)Fzl 

-  (zjl/Gl)Fyl  -  HDx  =  Tlx  -  TOx  (7) 

Combining  equations  (8a)  and  (8b)  yields: 


ZMly  =  (-zjO/Gl)FxO  +  (xjO/Gl)FzO  +  (zjl/Gl)Fxl 

-  (xjl/Gl)Fzl  -  HDy  =  Tly  -  TOy  (8) 

Combining  equations  (9a)  and  (9b)  yields: 

2Mlz  =  -(xjO/Gl)FyO  +  (yjO/Gl)FxO  +  (xjl/Gl)Fyl 

-  (yjl/Gl)Fxl  -  HDz  =  Tlz  -  TOz  (9) 

C.    LINK  TWO  EQUATIONS 

1 .  Sum  of  Forces  Equations 

From  the   free   body   diagram  (Figure  1)  it  follows 
that 
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ZFx  =  Fx2  -  Fxl  =  M2ax2  (10) 

ZFy  =  Fy2  -  Fyl  =  M2ay2  (11) 

ZFz  =  Fz2  -  Fzl  =  M2az2  (12) 

2 .  Joint  Equations 

Analysis  is  conducted  at  joint  one  where  similar 
equations  are  used  as  in  joint  zero  with  a  point  on  link  one 
(a)  and  one  on  link  two  (b).   For  point  a  the  equation  is 

Aa  =  Al  +  wdl  x  ra/Gl  +  wl  x  (wl  x  ra/Gl ) 

ra/Gl  is  a  vector  whose  distance  is  measured  from  point  a  to 
the  center  of  gravity  of  link  one  in  the  x,  y  and  z 
direction. 

ra/Gl  =  (jxl  -  LCOGxDi  +  ( jyl  -  LC0Gyl)j 
+  (jzl  -  LCOGzDk 
=  ra/Glx  +  ra/Gly  +  ra/Glz 

For  point  b  the  equation  is: 

Ab  =  A2  +  wd2  x  rb/G2  +  w2  x  (w2  x  rb/G2) 
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where  rb/G2  is  a  vector  whose  distance   is  measured  from 
point  b  to  the  center  of  gravity  of  link  two. 

rb/G2  =  (jxl  -  LC0Gx2)i  +  ( jyl  -  LC0Gy2)j 
+  (jzl  -  LC0Gz2)k 
=  rb/G2x  +  rb/G2y  +  rb/G2z 

Equating  Aa   and  Ab   and  setting  knowns  and  unknowns 
on  the  respective  sides  of  the  equation  results  in: 

Ax2  -  Axl  +  wdy2(rb/G2z)  -  wdz2(rb/G2y)  -  wdyl(ra/Glz) 
+  wdzl(ra/Gly)  =  MIC1  -  M1C2  (13) 

MIC1  =  wylwxl(ra/Gly)  -  (wyl )2(ra/Glx)  -  (wzl )2(ra/Glx) 
+  wzlwxl(ra/Glz) 

MIC2  =  wy2wx2(rb/G2y)  -  (wy2 )2(rb/G2x)  -  (wz2 ) 2 ( rb/G2x) 
+  wz2wx2(rb/G2z) 

Ay2  -  Ayl  +  wdz2(rb/G2x  -  wdx2(rb/G2z)  -  wdzl(ra/Glx) 
+  wdxl(ra/Glz)  =  MJC1  -  MJC2  (14) 

MJC1  =  wzlwyl(ra/Glz)  -  (wzl ) 2( ra/Gly )  -  (wxl )2( ra/Gly ) 
+  wxlwyl(ra/Glx) 
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MJC2  =  wz2wy2(rb/G2z)  -  (wz2 ) 2 ( rb/G2y )  -  (wx2 )2( rb/G2y ) 
+  wx2wy2(rb/G2x) 

AZ2  -  AZ1  +  wdx2(rb/G2y)  -   wdy2 ( rb/G2x)  -  wdxl(ra/Gly) 
+  wdyl(ra/Glx)  =  MKC1  -  MKC2  (15) 

MKC1  =  wxlwzl(ra/Glx)  -  (wxl ) 2(ra/Glz )  -  (wyl )2( ra/Glz ) 
+  wylwzl(ra/Gly) 

MKC2  =  wx2wz2(rb/G2x)  -  (wx2)2(rb/G2z )  -  (wy2 ) 2(rb/G2z ) 
+  wy2wz2(rb/G2y) 

3 .  Sum  of  the  Moment  Equations 

These  equations  have  a   similar  development   as  that 
of  link  one: 

2M2  =  (rjl/G2)  x  Fl  +  (rj2/G2)  x  F2  +  Tl  -  T2 

where 

rjl/G2  =  (xjl  -  xG2)i  +  (yjl  -  yG2)j  +  (zjl  -  zG2)k 
rj2/G2  =  (xj2  -  xG2 ) i  +  (yj2  -  yG2 ) j  +  (zj2  -  zG2)k 
2M2x  =  -  (yjl  -  yG2)Fzl  +  (zjl  -  zG2)Fyl 
+  (yj2  -  yG2)Fz2  -  (zj2  -  zG2)Fy2 
+  Tlx  -  T2x  (16a) 
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ZM2y  =  -  (zjl  -  zG2)Fxl  +  (xjl  -  xG2)Fzl 
+  (zj2  -  zG2)Fx2  -  (xj2  -  xG2)Fz2 
+  Tly  -  T2y  (17a) 

2M2z  =  -  (xjl  -  xG2)Fyl  +  (yjl  -  yG2)Fxl 
+  (xj2  -  xG2)Fy2  -  (yj2  -  yG2)Fx2 
+  Tlz  -  T2z  (18a) 

From  angular  momentum  equation   developed  for   link  one,  it 
can  be  shown  for  link  two: 

2M2x  =  HDx  (16b) 

2M2y  =  HDy  (17b) 

2M2z  =  HDz  (18b) 

Combining  equations  (16a)   and   (16b)  the  following 
result : 

-  (yjl  -  yG2)Fzl  +  (zjl  -  zG2)Fyl  +  (yj2  -  yG2)Fz2 

-  (zj2  -  zG2)Fy2  -  HDx  =  -  Tlx  +  T2x  (16) 

Combining   equations    (17a)   and   (17b)   yield   the 
following  result: 

-  (zjl  -  zG2)Fxl  +  (xjl  -  xG2)Fzl  +  (zj2  -  zG2)Fx2 

-  (xj2  -  xG2)Fz2  -  HDy  =  -Tly  +  T2y  *  (17) 
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Combining   equations   (18a)   and  (18b)   yield   the 
following  result: 

-  (xjl  -  xG2)Fyl  +  (yjl  -  yG2)Fxl  +  (xj2  -  xG2)Fy2 

-  (yj2  -  yG2)Fx2  -  HDz  =  -Tlz  +  T2z  (18) 

D.    LINK  THREE  EQUATIONS 

1 .  Sum  of  Forces  Equations 

Following   similar    logic   from  that   previously 
developed: 

2Fx  =  -  Fx2  =  M3ax3  (19) 

ZFy  =  -  Fy2  =  M3ay3  (20) 

SFz  =  -  Fz2  -  W3  =  M3az3  (21) 

2 .  Joint  Equations 

With  point  a  on  link  two  and  point   b  on   link  three 
one  gets  for  joint  equations  at  joint  two: 

Aa  =  A2  +  (wd2  x  ra/G2 )  +  w2  x  (w2  x  ra/G2) 

where  ra/G2  is  a  vector  whose  distance  is  measured  from 
point  a  to  center  of  gravity  of  link  two  in  the  x,  y  and  z 
direction. 
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ra/G2  =  ( jx2  -  LC0Gx2 ) i  +  ( jy2  -  LC0Gy2)j 
+  (jz2  -  LC0Gz2)k 
=  ra/G2x  +  ra/G2y  +  ra/G2z 

For  point  b 

Ab  =  A3  +  wd3  x  rb/G3  +  w3  x  (w3  x  rb/G3 ) 

where  rb/G3  is  a  vector  whose  distance  is  measured  from 
point  b  to  center  of  gravity  of  link  three  in  the  x,  y  and  z 
direction. 

rb/G3  =  (jx2  -  LC0Gx3 ) i  +  ( jy2  -  LC0Gy3)j 
+  (jz2  -  LC0Gz3)k 
=  rb/G3x  +  rb/G3y  +  rb/G3z 
Equating  Aa  and  Ab   and  setting  knowns  and  unknowns  on  the 
respective  sides  of  the  equation  results  in: 

Ax3  -  Ax2  +  wdy3(rb/G3z)  -  wdz3(rb/G3y)  -  wdy2(ra/G2z) 
+  wdz2(ra/G2y)  =  MIC3  -  MIC4  (22) 

MIC3  =  wy2wx2(ra/G2y)  -  (wy2 ) 2(ra/G2x)  -  (wz2 ) 2( ra/G2x) 
+  wz2wx2(ra/G2z) 

MIC4  =  wy3wx3(rb/G3y)  -  (wy3 )2( rb/G3x)  -  (wz3 )2( rb/G3x 
+  wz3wx3(rb/G3z) 
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Ay  3  -  Ay2  +  wdz3(rb/G3x)  -  wdx3(rb/G3z)  -  wdz2(ra/G2x) 
+  wdx2(ra/G2z)  =  MJC3  -  MJC4  (23) 

MJC3  =  wz2wy2(ra/G2z)  -  (wz2)2(ra/G2y)  -  (wx2)2(ra/G2y ) 
+  wx2wy2(ra/G2x) 

MJC4  =  wz3wy3(rb/G3z)  -  w2z3(rb/G3y)  -  w2x3(rb/G3y) 
+  wx3wy3(rb/G3x) 

AZ3  -  AZ2  +  wdx3(rb/G3y)  -  wdy3(rb/G3x)  -  wdx2(ra/G2y) 
+  wdy2(ra/G2x)  =  MKC3  -  MKC4  (24) 

MKC3  =  wx2wz2(ra/G2x)  -  (wx2)2(ra/G2z)  -  (wy2)2(ra/G2z ) 

+  wx2wy2(ra/G2y) 

MKC4  =  wx3wz3(rb/G3x)  -  (wx3 )2(rb/G3z)  -  (wy3 )2(rb/G3z ) 

+  wy3wz3(rb/G3y) 

3 .  Sum  of  Moment  Equations 

As  in  the  development  of  the  equations  for  link  one: 

2M3  =  (rj2/G3)  x  F2  +  T2 

where 

rj2/G3  =  (xj2  -  xG3)i  +  (yj2  -  yG3)j  +  (zj2  -  zG3)k 
=  XJ2/G3  +  yj2/G3  +  zj2/G3 
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2M3x  =  (-yj2/G3)Fz2  +  (zj2/G3)Fy2  +  T2x         (25a) 


ZM3y  =  (-zj2/G3)Fx2  +  (xj2/G3)Fz2  +  T2y         (26a) 


ZM3z  =  (-xj2/G3)Fy2  +  (yj2/G3)Fx2  +  T2z         (27a) 


From  the  angular  momentum  theory: 


2M3x  =  HDx  (25b) 


2M3y  =  HDy  (26b) 


2M3z  =  HDz  (27b) 


Combining  equations  (25a)  and  (25b)  the  following  results: 


(-yj2/G3)Fz2  +  (zj2/G3)Fy2  -  HDx  =  -  T2x         (25) 


Combining  equations  (26a)  and  (26b)  the  following  results: 


(-zj2/G3)Fx2  +  (xj2/G3)Fz2  -  HDy  =  -  T2y         (26) 


Combining  equations  (27a)  and  (27b)  the  following  results: 


(-xj2/G3)Fy2  +  (yj2/G3)Fx2  -  HDz  =  -  T2z         (27) 
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III.  COMPUTATIONAL  APPROACH 

A.    PROGRAM  MATRICIES 

The  Dynamic  Simulation  Language  (DSL)  was  used  to 
simulate  the  motion.  This  computer  code  was  compiled  on  an 
IBM  3033  computer  by  using  the  FORTVS  compiler  and  all  of 
the  calculations  have  been  done  in  double  precision.  The 
entire  simulation  process  is  shown  in  Figure  2  and  is 
discussed  below. 

The  principle  program  matrix,  Matrix  A  (MATA,  27*27), 
was  created  from  the  coefficients  of  the  unknown  variables 
in  equations  1  to  27.  In  the  simulation  of  the  direct 
dynamics  problem,  a  corresponding  27*1  Matrix  B  (MatB)  was 
generated  from  all  known  variables,  also  from  equations  1  to 
27.  A  subroutine  CPROD  was  used  to  perform  all  the  cross 
product  terms  required  in  the  main  program.  The  resulting 
equations  are  shown  in  Figure  3,  in  the  final  matrix  form. 
During  a  simulation  time  step,  the  link  inertias,  the  link 
velocities  and  the  link  positions  were  all  assumed  constant. 
IMSL  subroutine  LEQT2F  was  called  to  invert  the  matrix  A  and 
get  the  generalized  solution  x  from  Ax  =  B.  This  subroutine 
uses  Gaussian  elimination  with  iterative  improvement  to  get 
a  high  accuracy  solution  to  the  problem.  The  output  from 
LEQT2F  then  returns  as  MATB,  which  contains  the  solution  to 
the  equations.   The  outputs  were  used  by  DSL  to  integrate 
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the  linear  and  angular  accelerations  of  each  link  to  get  the 
linear  and  angular  velocities  respectively.  The  linear 
velocities  for  each  link  were  next  integrated  to  get  the 
linear  displacements  of  center  of  gravity  of  each  link. 
Although  the  linear  velocities  in  the  fixed  reference  frame 
can  be  integrated  to  get  the  linear  displacements,  this  idea 
is  not  true  for  the  angular  displacements  [Refs.  20,  21]. 

To  get  the  angular  displacements,  a  set  of 
transformation  matrices  must  be  used  on  the  velocities,  then 
the  motion  can  be  integrated.  That  is,  the  angular 
velocities  of  each  link  in  the  fixed  reference  system  must 
first  be  converted  into  equilavences  in  a  body  fixed 
coordinate  system  then  into  body  Euler  rates  and  Euler 
angles  to  define  the  motion  unambigously .  In  this  thesis, 
the  body  coordinate  velocities  are  called  Bratel,  Brate2  and 
Brate3  for  the  link  one,  link  two  and  link  three 
respectively.  To  convert  these  velocities  into  the  Euler 
rates,  another  transformation  matrix  is  used.  That  is,  the 
transformation  matrix  is  multiplied  by  body  rates  to  get  the 
Euler  angle  rates  for  each  link.  These  later  rates  are 
defined  as  the  Yaw  rate  (about  the  x  axis),  the  Pitch  rate 
(about  the  y  axis)  and  the  Roll  rate  (about  z  axis).  These 
rates  are  called  as  Ratel,  Rate2  and  Rate3  for  link  one, 
link  two  and  link  three.  After  the  transformation  of 
velocities  to  the  Euler  rates,  they  can  be  directly 
integrated  to  get  the  Euler  angles.   In  this  thesis,  these 
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angles  are  called  the  Yaw,  pitch  and  Roll  angles  about  the  x 
y   z   axis  respectively  [Refs.  2,  20]. 

This  convention  is  very  important  and  should  not  be 
mixed  with  another  set  of  Euler  angles  described  differently 
in  the  literature  [Refs.  3,  20].  In  addition  to  that,  the 
order  of  the  rotation  must  be  decided  beforehand.  This  is 
true  because  the  orientation  of  objects  is  different  when 
they  are  rotated  in  a  different  order,  i.e.,  first  the 
rotation  about  x  axis,  then  a  rotation  about  the  y  axis, 
finally  a  rotation  about  the  z  axis  will  produce  a  different 
orientation  in  space  than  the  one  which  was  defined  and  used 
in  this  thesis  (z,  y,  then  x).  The  transformation  matrices 
used  here  are  valid  as  long  as  the  assumed  order  of  the 
rotation  is  retained. 

In  the  literature,  a  quite  different  set  of  angles  is 
used  to  describe  the  orientation  [Refs.  2,  3].  While  some 
of  these  angles  define  the  orientation  with  respect  to  a 
non-orthogonal  coordinate  system  some  others  may  define  with 
respect  to  an  orthogonal  system.  Euler  angles  define  an 
independent  set  of  coordinates  system  which  are  not 
orthogonal.  Therefore,  all  three  coordinates  are 
independent  from  each  other  and  velocities  in  this 
coordinate  system  can  be  directly  integrated  to  get  the 
relevant  angles.  They  describe  the  unique  orientation  of 
the  body  in  space.  The  orthogonal  set  of  coordinate  axes  do 
not  form  an  independent  coordinate   system.     This   is  true 
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since  the  three  axis  have  a  certain  relation  with  each  other 
in  any  position,  i.e.,  direction  cosine  angles  have  a  unique 
relation  in  a  fixed  reference  system  and  cannot  be  obtained 
by  integrating  any  velocity  in  an  orthogonal  coordinate 
system.  The  velocities  in  an  orthogonal  coordinate  system 
must  thus  be  converted  to  a  nonorthogonal  coordinate  system 
(e.g.  Euler  angle  rates)  prior  to  integration. 

After  Yaw,  Pitch  and  Roll  angles  are  calculated,  it  is 
possible  to  go  back  and  express  the  orientation  of  the  body 
with  the  direction  cosines  in  an  orthogonal  coordinate 
system.  The  columns  of  the  transformation  matrix  from  one 
orthogonal  set  of  axes  to  another  describes  the  orientation 
of  the  new  coordinate  axis  with  respect  to  old  coordinate 
system.  So,  a  transformation  matrix  can  be  used  to  get  the 
direction  cosine  angles.  The  direction  cosines  of  each  link 
are  then  used  to  calculate  the  moment  of  inertia  of  the 
links.  The  variation  of  a  link  inertia  with  respect  to  time 
was  shown  in  Figure  4  as  it  was  calculated  during  a 
simulation  run.  The  derivation  of  the  transformation 
matrices  is  shown  in  Appendix  A. 

B.    CONSTRAINTS  IN  THE  SIMULATION  PROGRAM 

In  the  development  of  the  equations,  thus  far,  each 
link  has  been  treated  such  that  it  can  move  in  space  without 
any  constraint.   For  most  cases,  however,  degrees  of  freedom 
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of  each  link  must  be  reduced  so  that  the  link  can  move  only 
in  the  direction  permitted  by  its  joint. 

In  the  simulation  of  the  direct  dynamics  problem,  the 
base  rotation  is  transmitted  to  the  second  and  third  links 
for  the  three  revolute  joint  arm  which  was  studied.  This 
was  simulated  by  allowing  the  first  link  to  rotate  only 
about  Z  axis.  At  the  same  time,  the  rotational  rates  of  the 
second  and  third  links  about  the  Z  axis  were  made  equal. 

To  make  any  of  the  simulation  variables  zero,  meaning 
no  variation  in  that  direction,  one  zeroes  the  related  rows 
and  columns  in  MatA  putting  1  on  the  diagonal.  At  the  same 
time,  if  the  same  row  in  MatB  is  made  zero,  the 
corresponding  mathematical  expression  for  this  equation  will 
be  in  the  form  of  1  *  X  =  0 ,  and  a  result  of  this,  X  will  be 
equal  zero.  This  idea  can  also  be  applied  to  MatA  and  MatB 
to  make  two  variables  equal  so  that  XI  -  X2  =  0 .  Thus,  the 
above  motion  was  simulated  by  constraint. 

C.    PROGRAM  VALIDATION 

The  validation  of  the  inverse  dynamics  problem  has  been 
conducted  in  several  cases.  In  this  approach  the  idea  was 
to  choose  an  angular  acceleration  such  that  at  a  certain 
time,  two  of  the  three  links  would  align.  In  other  words, 
the  links  would  be  in  a  singular  position  at  this  time,  and 
if  the  simulation  procedure  worked,  the  singularity  problem 
would  have  been  avoided. 
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The  validation  procedure  is  shown  in  Figure  5.  For 
this  procedure  link  two  angle  was  chosen  as  0  =  (Pi/2)  * 
sin (pi/2)  *  Time.  This  time  dependent  function  has  a  period 
of  4  sec  and  an  amplitude  of  90  degrees.  The  second 
derivative  of  this  function  is  9  =  -  ((pi**3)/8)  *  sin(pi/2) 
*  Time  and  corresponds  to  the  angular  acceleration  of  the 
link.  This  value  was  input  as  the  theoretical  angular 
acceleration  in  the  simulation  program,  and  corresponding 
linear  accelerations  and  forces  at  each  joint  were 
calculated.  The  other  two  links  were  forced  to  have  zero 
rotational  velocity  throughout  the  simulation. 

To  apply  a  corresponding  torque  at  the  joint,  MaTA  and 
MatB  were  multiplied  and  a  right  hand  side  matrix  DQ  (27*1) 
was  obtained  (MATA  *  MATB  =  DQ) .  This  matrix  DQ  (27*1)  was 
used  to  solve  the  simulation  equations  in  the  form  of 
AX  =  DQ.  The  vector  X  (that  is,  theta)  was  fedback  in  the 
loop  and  the  theoretical  and  the  calculated  values  of  theta 
were  compared. 

The  above  discussion  has  been  implemented  in  three 
different  initials  configuration  as  shown  in  Figure  6.  To 
force  the  arm  links  to  the  various  singular  points,  several 
different  plane  motions  were  simulated.  For  each 
configuration,  three  different  angular  motions  were  input 
for  link  2,  or  as  can  be  seen  from  Figure  6,  for  each 
configuration,  one  angular  velocity  caused  a  spinning  motion 
of  the  link  about  the  axis  with  which  it  was  initially 
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aligned,  while  the  other  two  produced  a  plane  motion 
according  to  direction  of  the  applied  angular  motion.  The 
angles  between  two  successive  links  were  measured  for  each 
motion.  Figure  7  shows  the  angle  variation  between  link  1 
and  link  2,  and  link  2  and  link  3  corresponding  to  an 
angular  acceleration  applied  in  the  X  direction  for 
configuration  A.  As  can  be  seen  from  Figures  7  and  8,  two 
successive  links  pass  through  the  singular  points  every  2 
seconds,  i.e.,  they  align  and  the  angle  between  links 
becomes  either  0  or  180  degrees.  (The  singular  points  are 
marked  on  the  graph) .  Figure  8  shows  the  angle  variations 
for  an  angular  motion  applied  in  the  Z  direction  for 
configuration  A.  In  this  case,  it  is  obvious  that  the  angle 
between  link  1  and  link  2  is  always  constant  (90  degrees). 
The  second  graph  on  Figure  8  shows  the  angle  between  link  2 
and  link  3  now,  singularity  occurs  on  the  Z  motion,  with  the 
singularities  marked  as  in  Figure  8.  Figure  7  and  Figure  8 
are  representative  of  the  data  obtained  in  the  validation 
procedure  which  analyzed  nine  possible  motions  of  link  2 
leading  the  singularity.  This  data  showed  that  singularity 
in  these  directions  could  be  overcome,  and  a  solution  to  the 
problem  exists  using  this  approach. 

For  each  run,  the  error  between  the  theoretical  and  the 
simulated  value  of  Theta  was  computed.  Figure  9  shows  the 
percent  error  for  the  X  motion  for  configuration  A  (Figure  7 
Data).   The  trend  of  the  error  is  representative  of  every 
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case  investigated.  The  figure  shows  that,  due  to  nature  of 
the  numerical  integration,  the  error  slightly  accumlates 
during  the  simulation,  but  still  has  very  small  value.  This 
proves  that  the  direct  dynamics  problem  can  be  solved  very 
accurately  by  Newton-Euler  approach  in  a  fixed  coordinate 
system. 
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IV.  RESULTS  AND  RECOMMENDATIONS 

A  dynamic   model  of  a  three  link,  rigid  revolute  joint 

manipulator  has  been  developed  in  this  thesis,  as  a 

general  computer  program  package. 

Several  runs   for  different  initial  configurations  were 

simulated  and  the  singularity  problem  was  investigated. 

Theoretical  and   calculated  values  of  angular  positions 

were  compared.    It  was   proved  that   the  singularity 

problem  could  be   overcome   by  using  a  Newton-Euler 

approach  in  a  fixed  coordinate  system. 

The  following  recommendations  are  provided: 

a.  Enhance  the  code  and  make  it  more  interactive. 
That  is,  let  the  user  specify  the  constraints  he 
wants  to  apply  on  each  link  by  answering 
interactive  questions  before  the  actual  simulation 
run  starts.  Thus,  the  motion  can  be  simulated 
with  different  constraints  without  going  into  the 
code  and  changing  the  relevant  parameters. 

b.  Adapt  the  code  for  use  in  a  microcomputer.  Add  a 
subroutine  in  the  program  to  invert  the  matrix  A. 
Thus,  the  code  will  be  more  independent  from 
outside  routines  and  more  adaptable  to  other 
computer  systems. 
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Validation  of  the  approach  via  actual  experimental 
tests  in  crucial.  This  will  establish  a  way  of 
developing  accurate  constants  for  subsequent 
controller  design  and  provide  a  basis  for 
compensation  of  gravity  effects.  Determining 
these  constants  for  the  code  will  make  the 
simulation  program  more  concrete  and  will  provide 
more  physical  insight. 

Finally,  develop  a  controller  for  a  manipulator 
which  makes  use  of  the  present  algorithm  for 
validation  and  design. 
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APPENDIX  A 

DERIVATION  OF  THE  TRANSFORMATION  MATRIX  FROM 
EARTH  FIXED  COORDINATE  SYSTEM  TO  BODY  FIXED  COORDINATE  SYSTEM 

The  angular  velocity  terms  obtained  by  integration  of 
the  angular  acceleration  terms  are  with  respect  to  an  Earth 
fixed  coordinate  system.  To  define  the  Euler  angles  which 
are  called  Yaw  Pitch  and  Roll  angles  in  this  thesis,  we 
have  to  establish  an  appropriate  body  fixed  coordinate 
system.  Thus,  U,  V  and  W  is  a  right  hand  coordinate  system 
[Ref .  20]  with  its  origin  fixed  at  the  center  of  gravity  of 
a  link.  The  U,  V,  W  coordinate  system  is  initially  oriented 
such  that  the  angles  between  two  coordinate  system  axes  are 
simultaneously  reduced  to  zero,  i.e.,  i,  j,  k,  axis  are 
parellel  to  the  I,  J,  K  respectively. 

If  a  rotation  from  X  Y  Z  coordinate  system  to  the  U  V  W 
coordinate  system  is  accomplished  by  first  rotation  about  K 
axis  (roll),  then  about  J  axis  (pitch)  and  finally  about  I 
axis  (yaw),  it  follows  that  for  any  arbitrary  point  in  the  X 
Y  Z  coordinate  system,  the  corresponding  coordinates  in  the 
U  V  W  system  are; 
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where  MatR  is  a  3*3  matrix. 

To  get  the  transformation  matrix,  we   need  to   examine  each 
rotation  separately. 

Rotation  about  the  Z  axis; 
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Rotation   about  the  Y  axis; 
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Rotation  about  the  X  axis; 


Vs  Yctp  +  zsy 
w.-Ysy+zcy 


X,u 


u 

{       O         0 

X 

V 

— 

o    cy    sy 

Y 

vV 

o    -s^   c-V 

2 

-    _ 

k_                 — 

—    «. 

By  multiplying  three  rotation  matrix  together; 

cgc$  ces<j>        -se 

where     C  =  COS 

S  =  Sin 

T  =  Tan 
The   transformation   matrix   from  body   fixed  to  Euler 
coordinate  system  is  obtained  as  below  [Ref.  20]. 

*  T**V  T9CJ* 

0        ScceSf       SecBcy 

The  angles  discussed  above  are  shown  in  Figure  10 
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Figure  10.  Critical  Angles 

(a)  Euler  Angles,  Body  Coordinates 

(b)  Direction  Cosines,  Global  Coordinates 
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APPENDIX  B 
THREE  DIMENSIONAL  DIRECT  DYNAMICS  SIMULATION  PROGRAM 

TERMINAL 

METHOD  ADAMS 

PRINT  .05,DRCANX(l-3),DRCANY(l-3),DRCANZ(l-3)/... 

JXO , JYO , JZO , JX1 , JY1 , JZ1 , JX2 , JY2 , JZ2 

LCOGX(l-3) ,LCOGY(l-3) ,LCOGZ(l-3) 

CONTROL  FINTIM  =2.0,  DELMAX  =.1,  DELPRT  =  .05 

D     DIMENSION  MATA(27 . 27 ) ,MASS (3 , 2) , L(3 . 2) ,RX(3 , 2) ,RY(3 , 2) ,RZ(3 , 2) 

D     DIMENSION  IXX(3 , 2) , IXZ(3 , 2) , IXY(3 , 2) , IYY(3 , 2) , IYZ(3 , 2) , IZZ(3 , 2) 

D     DIMENSION  MAT1R(3 , 3 ) ,MAT2R(3 , 3) ,MAT3R(3 , 3 ) 

D     DIMENSION  MAT1T(3 , 3) ,MAT2T(3 , 3 ) ,MAT3T(3 , 3) 

D      INTEGER  IER, I , J ,M,K,P,N/ IA, IDGT, A 

EXCLUDE  IA,IDGT,IER,I,J,M,K,P,N,A 

ARRAY  MATB(27) ,LCOGX(3) , LCOGY(3) ,LCOGZ(3) 

ARRAY  VECTAO ( 3 ) , VECTBO ( 3 ) . VECTA1 ( 3 ) , VECTB1 ( 3 ) , VECTA2 ( 3 ) , VECTB2 ( 3 ) 

ARRAY  WDX(3),WDY(3),WDZ(3),W1(3),W2(3),W3(3) 

ARRAY  RATE1(3),RATE2(3),RATE3(3),BRATE1(3)/BRATE2(3),BRATE3(3) 

ARRAY  RBG1(3),RAG1(3),RBG2(3) ,RAG2 (3 ) , RBG3 (3) 

ARRAY  SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 

ARRAY  IXXT ( 3 ) , IYYT ( 3 ) , IZZT ( 3 ) , IXYT ( 3 ) , IXZT ( 3 ) , IYZT ( 3 ) 

ARRAY  YAWANX ( 3 ) , PTCANY ( 3 ) , ROLANZ ( 3 ) 

ARRAY  DRCANX ( 3 ) , DRCANY ( 3 ) , DRCANZ ( 3 ) 

ARRAY  DRCRAX ( 3 ) , DRCRAY ( 3 ) , DRCRAZ ( 3 ) 

ARRAY  DRCSX(3) ,DRCSY(3),DRCSZ(3) 

D     DATA  MATA/729  ^  0.0D0/ 

INITIAL 

*  INPUT  PARAMETER  CONSTANTS 


PI 


A  = 

5 

,0D0 

P  = 

0 

.0D0 

-w  = 

2 

.0D0  * 

IDGT  = 

--   3 

G=0. 

0D0 

N=2" 

r 

M=l 

IA  = 

=27 

:  JOINT  I 

,OCA 

JXO 

= 

0 

0D0 

JYO 

= 

0 

0D0 

JZO 

= 

0 

0D0 

JX1 

= 

0 

0D0 

JY1 

= 

0 

0D0 

JZ1 

= 

1 

0D0 

JX2 

= 

0 

0D0 

JY2 

= 

1 

.0D0 

JZ2 

= 

1 

-0D0 

:  TORQUE 

CON 

TOX 

= 

0 

.0D0 

TOY 

= 

0 

0D0 

TOZ 

= 

0 

,0D0 

T1Y 

= 

0 

.0D0 

T1Z 

= 

0 

.0D0 

T2Y 

= 

0 

,0D0 

T2Z 

= 

0 

.0D0 

INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS 
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FOR  EACH  LINK  ENDS 


1,1, 

1,2, 

2,1 
2,2' 
3,1, 
3,2 


0.50D0 
0.50D0 
0.50D0 
0.50D0 
0.50D0 
0.50D0 


INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 


MASS (1,1' 
MASS(1,2' 
MASS (2,1' 
MASS (2, 2' 
MASS(3,1' 
MASS(3,2' 


5DO 
5D0 
5D0 
5D0 
5D0 
5D0 


INPUT  OMEGA  AND  OMEGA  DOT,  YAW,  PITCH,  AND  ROLL  ANGLES 
DO  30  I  =  1,3 


W1(I) 
W2(I) 
W3(I) 
WDX(I 
WDY(I 
WDZ(I 


0.0D0 
O.ODO 
0.0D0 
O.ODO 
O.ODO 
O.ODO 


YAWANX(I)  =  O.ODO 
PTCANY(I)  =  O.ODO 
ROLANZ(I)  =  O.ODO 


30 


CONTINUE 

YWRX1 
PTRY1 
RLRZ1 
YWRX2 
PTRY2 
RLRZ2 
YWRX3 
PTRY3 
RLRZ3 


YAWANXI 
PTCANYl 
ROLANZ I 
YAWANXI 
PTCANYl 
ROLANZ l 
YAWANXI 
PTCANYI 
ROLANZ l 


*  DEGRA 

*  DEGRA 

*  DEGRA 

*  DEGRA 

*  DEGRA 

*  DEGRA 

*  DEGRA 

*  DEGRA 

*  DEGRA 


INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 


LCOGX(l 

XI 

LCOGY(l 

Yl 

LCOGZ(l 

Zl   = 

LCOGX(2 

X2 

LCOGY(2 

Y2 

LCOGZ(2 

22   - 

LCOGX(3 

X3   = 

LCOGY(3 

Y3 

LCOGZ(3 

Z3 


)  =  O.ODO 

LCOGX(l) 
)  =  O.ODO 

LCOGY(l) 
)  =  0.5D0 

LCOGZ(l) 
)  =  O.ODO 

LCOGX(2) 
)  =  0.5D0 

LCOGY(2) 
)  =  1.0D0 

LCOGZ(2) 
)  =  O.ODO 

LCOGX(3) 
)  =  1.5D0 

LCOGY(3) 
)  =  1.0D0 

LCOGZ(3) 


INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 

MASS1  =  5.0D0 
MASS2  =  5.0D0 
MASS3  =  5.0D0 

WG1  =  MASSING 
WG2  =  MASS2*G 
WG3  =  MASS3*G 

INPUT  ACCELERATION  OF  JOINT  ZERO 
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AOX  =  O.ODO 
AOY  =  O.ODO 
AOZ  =  O.ODO 

*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  40  I  =  1,27 
DO  50  J  =  1,27 

MATA(I,J)  =  O.ODO 
50  CONTINUE 

40        CONTINUE 

DO  60  I  =  1,27 
MATB(I)  =  O.ODO 
60        CONTINUE 

*  INITIALIZE  THE  TRANSFORMATION  MATRICIES  AND  VELOCITIES 

DO  63  I  =  1,3 
DO  64  J  =  1,3 

RATEl(I)  =  O.ODO 

RATE2(I)  =  O.ODO 

RATE3(I)  =  O.ODO 

BRATEl(I)  =  O.ODO 

BRATE2(I)  =  O.ODO 

BRATE3(I)  =  O.ODO 

MAT1T  (I, J)  =  O.ODO 

MAT2T  (I, J)  =  O.ODO 

MAT3T  (I, J)  =  O.ODO 

MAT1R  (I, J)  =  O.ODO 

MAT2R  (I, J)  =  O.ODO 

MAT3R  (I, J)  =  O.ODO 

64  CONTINUE 

63       CONTINUE 

DERIVATIVE 
NOSORT 

CALL  ERRSET  (208,256,-1,1,1) 

LEVELQ  =  0 

CALL  UERSET( LEVELQ, LEVLDQ) 

*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  70  I  =  1,27 
DO  80  J  =  1,27 

MATA(I,J)  =  O.ODO 
80  CONTINUE 

70        CONTINUE 

DO  90  I  =  1,27 
MATB(I)  =  O.ODO 
90        CONTINUE 

*  INPUT  JOINT  EQUATIONS 

*  JOINT  ZERO   EQUATIONS 

*  AB  =  AG1  +  (WD1  X  RB/G1)  +  Wl  X  (Wl  X  RB/G1) 

VECTAO(l)  =  WDX(l) 
VECTAO (2)  =  WDY(l) 
VECTAO (3)  =  WDZ(l) 


RBG1(1)  =  JXO  -  LCOGX(l) 
RBG1(2)  =  JYO  -  LCOGY(l) 
RBG1(3)  =  JZO  -  LCOGZ(l) 

CALL  CPROD( VECTAO, RBG1,MIA0,MJA0,MKA0) 

VECTAO' 
VECTAO 


VECTAO i 
CALL  CPROD( VECTAO, RBG1 ,MIBO ,MJBO ,MKBO) 
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VECTBO(l)  =  MIBO 
VECTB0(2)  =  MJBO 
VECTB0(3)  =  MKBO 

CALL  CPROD ( VECTAO , VECTBO , MI CO , M JCO , MKCO ) 

*  JOINT  ONE  EQUATIONS --- 

*  AA  =  AG1  +  (WD1  X  RA/G1)  +  Wl  X  (Wl  X  RA/G1) 

VECTAl(l)  =  WDX(l) 
VECTA1(2)  =  WDY(l) 
VECTA1(3)  =  WDZ(l) 

RAG1(1)  =  JX1  -  LCOGX(l) 
RAG1(2)  =  JY1  -  LCOGY(l) 
RAG1(3)  =  JZ1  -  LCOGZ(l) 

CALL  CPROD (VECTA1 ,RAG1 ,MIA1 ,MJA1 ,MKA1 ) 

VECTAl(l)  =  Wl(l) 
VECTA1(2)  =  Wl{2) 
VECTA1(3)  =  Wl(3) 

CALL  CPROD  (VECTA1,RAG1,MIB1,MJB1,MKB1) 

VECTBl(l)  =  MIB1 
VECTB1(2)  =  MJB1 
VECTB1(3)  =  MKB1 

CALL  CPROD  (VECTA1,VECTB1,MIC1,MJC1,MKC1) 

*  AB  =  AG2  +  (WD2  X  RB/G2)  +  W2  X  (W2  X  RB/G2) 

VECTAl(l)  =  WDX(2) 
VECTA1(2)  =  WDY(2) 
VECTA1(3)  =  WDZ(2) 

RBG2(1)  =  JX1  -  LCOGX(2) 
RBG2(2)'=  JY1  -  LCOGY(2) 
RBG2(3)  =  JZ1  -  LCOGZ(2) 

CALL  CPROD  (VECTA1,RBG2,MIA2,MJA2,MKA2) 

VECTAl(l)  =  W2(l) 
VECTA1(2)  =  W2(2) 
VECTA1(3)  =  W2(3) 

CALL  CPROD  (VECTA1/RBG2,MIB2,MJB2,MKB2) 

VECTBl(l)  =  MIB2 
VECTB1(2)  =  MJB2 
VECTB1(3)  =  MKB2 

CALL  CPROD  (VECTA1,VECTB1,MIC2,MJC2,MKC2) 

*  JOINT  TWO  EQUATIONS 

*  AA  =  AG2  +  (WD2  X  RA/G2)  +  W2  X  (W2  X  RA/G2) 

VECTA2(1)  =  WDX(2) 
VECTA2(2)  =  WDY(2) 
VECTA2(3)  =  WDZ(2) 

RAG2(1)  =  JX2  -  LCOGX(2) 
RAG2(2)  =  JY2  -  LCOGY(2) 
RAG2(3)  =  JZ2  -  LCOGZ(2) 

CALL  CPROD  (VECTA2,RAG2,MIA3,MJA3,MKA3) 

VECTA2(1)  =  W2(l) 
VECTA2(2)  =  W2(2) 
VECTA2(3)  =  W2(3) 

CALL  CPROD  (VECTA2,RAG2,MIB3,MJB3,MKB3) 

VECTB2(1)  =  MIB3 
VECTB2(2)  =  MJB3 
VECTB2(3)  =  MKB3 

CALL  CPROD (VECTA2 , VECTB2 , MI C3 , M JC3 , MKC3 ) 

*  AB  =  AG3  +  (WD3  X  RB/G3)  +  W3  X  (W3  X  RB/G3) 
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VECTA2(1 

VECTA2(2' 

VECTA2(3i 

RBG3(1)  ■■ 
RBG3(2)  : 
RBG3(3)  = 


=  WDX(3) 
=  WDY(3) 
=  WDZ(3) 

JX2  -  LC0GX(3' 
JY2  -  LC0GY(3' 
JZ2  -  LC0GZ(3' 


CALL  CPROD  (VECTA2,RBG3,MIA4,MKA4,MKA4) 


VECTA2I 

[1] 

i  s  W3< 

[1) 

VECTA2I 

[2 

i    =  W3< 

2) 

VECTA2I 

,3; 

'    =  W3< 

3) 

CALL  CPROD  (VECTA2,RBG3,MIB4,MJB4,MKB4) 


VECTB2(1' 
VECTB2(2' 
VECTB2(3' 


=  MIB4 
=  MJB4 
=  MKB4 


CALL  CPROD  (VECTA2,VECTB2,MIC4,MJC4,MKC4) 

SUM  OF  MOMENTS  EQUATIONS 
DO  100  I  =  1,3 
COMPUTE  HX,H  DOT  X,HY,H  DOT  Y,  HZ,H  DOT  Z 


rx(i,i; 

I    =   -LI 

!i*i) 

RX(I,2 

i    =      LI 

I1'2) 

RY(I,1 

i    =   -LI 

X1 

RY(I,2 

i    =      LI 

1,2 

RZ ( 1 , 1 

i    =   -LI 

1.1) 

RZ ( 1 , 2  j 

i    =      LI 

1,2) 

DCOS(DRCRAXl 
DCOSIDRCRAXI 
DCOSIDRCRAYi 
DCOSIDRCRAYi 
DCOSlDRCRAZi 
DCOSIDRCRAZi 


IXX(I,1)  =  MASS(I,1)  *  ((RY(I,1 

IXX(I,2)  =  MASS(l,2)  *  ((RY(I.2 

IXXT(I)   =  IXX(I,1)  +  IXX(I,2) 

IF  (IXXT(I)  .LE.  .020)  THEN 

IXXT(I)   =  .020 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 

IXY(I,1)  =  MASS 


RY(I,1))  + 
RY(I,2))  + 


IXY(li2)  = 
IXYT(I) 

IXZ(I,1 
IXZ(I,2 
IXZT(I) 

HDX(2)  = 
IYY 


m 


1,2' 


*  RX(I,1 
MASS(I,2)  *  RX(I,2 
IXY(I,1)  +  IXY 

MASS (1,1)  *  RZ 
MASS (I, 2)  *  RZ 
IXZ(I,1)  +  IXZ 

HDX(l)    =  WDX(l)    *   IXX(I,1)-   WDZ(I) 

IXX(I,2)-   WDZ(I) 
* 


RY(I,1) 
RY(I,2) 

K(I1J 


(RZ(I,1) 
(RZ(I,2) 


RZ(I 


■M 


WDX(2 

=  MASS(I,1 


*    IXZ(I,1 


■}) 


r(I) 


*   IXY(I, 


*    IXZ(I,2)    -   WDY(I)    *   IXY(I 


■}] 


1,1  =  MASS  1,1      RX  1,1 

IYY(I,2)  =  MASS(I,2)  *  ((RX(I,2) 

IYYT(I)  =  IYY(I,1)  +  IYY(I,2) 
IF  (IYYT(I)  .LE.  .020)  THEN 

IYYT(I)  =  .020 
ELSE 

IYYT(I)  =  IYYT(I) 
END  IF 


RX(I,1))  + 
RX(I,2))  + 


(RZ(I,1) 
(RZ(I,2) 


*mim 


IYZ(I,1)  =  MASS (1,1 
IYZ (I, 2)  =  MASS (1,2 


RY(I,1 
RY(I,2' 
IYZT(I)'  =  IYZ(I,1)'+  IYZ(I,2; 

HDY(l)  =  WDY(I)  *  IYY(I,1) 
HDY(2)  =  WDY(I)  *  IYY(I,2) 


;(i,i) 
;(i,2) 


*  RZ(I,1 

*  RZ! 


WDX(I)  *  IXY(I,1 
WDX(l)  *  IXY(I,2 


IZZ(I,1)  =  MASS (1,1 
IZZ(I,2)  =  MASS (I, 2 


(RX(I,1 
( RX (I  2 

IZZT(I)'  =  *izz(i*i)'+  IZZ(I,2J 

IF  (IZZT(I)  .LE.  .020)  THEN 


*  RX(I,1 


RX( 


l'2))    + 


-  WDZ(I 

-  WDZ(I 

RY(I,1) 
RY(I,2) 


IYZ 

IYZ 


*  RY(I 


SB! 
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IZZT(I)   =  .020 

ELSE 

IZZT(I)   =  IZZT(I) 

END  IF 


HDZ(l)  =  WDZ(I)  *  IZZ(I,1)  "  WDX(I)  *  IXZ(I,1)  -  WDY(I)  *  IYZ(I,1) 
HDZ(2)  =  WDZ(I)  *  IZZ(I,2)  -  WDX(I)  *  IXZ(I,2)  -  WDY(I)  *  IYZ(I,2) 


SUMHDXI 

[I] 

»  =  hdx(i; 

I  + 

HDXI 

[2) 

SUMHDY< 

I 

i  =  HDY(1 

1  + 

HDYI 

2) 

SUMHDZ< 

[i) 

i  =  HDZ(lj 

i  + 

HDZI 

\2) 

100     CONTINUE 

*  ENTER  CONSTANTS  INTO  MATRIX  A 

*  'LINK  ONE 

*  SUM  OF  FORCES  IN  THE  X  DIRECTION 

MATA(1,1)   =  1.0D0 

MATA(1,4)   =  MASS1 

MATA(l.lO)  =  -1.0D0 

MATB(l)    =  0.0D0 

*  SUM  OF  FORCES  IN  Y  DIRECTION 

MATA(2,2)  =  1.0D0 
MATA(2,5)  =  MASS1 
MATA(2,11)  =  -1.0D0 

MATB  ( 2 )     =   0  .  0D0 

*  SUM  OF  FORCES  IN  Z  DIRECTION 

MATA(3,3)  =  1.0D0 
MATA^e)  =  MASS1 
MATA(3,12)  =  -1.0D0 

*  SUM  OF  FORCES  LINK  ONE  EQUAL 

MATB (3)     =  -WG1 

*  EQUATIONS  AT  JOINT  ZERO 

*  IN  THE  X  DIRECTION 

MATA(4,4)  =   1.0D0 
MATA(4,8)  =  RBG1(3) 
MATA(4,9)  =  -RBG1(2) 

MATB(4)    =  AOX  -  MICO 

*  IN  THE  Y  DIRECTION 

MATA(5,5)  =   1.0D0 
MATA(5,7)  =  -RBG1(3) 
MATA(5;9)  =  RBG1(1) 

MATB (5)   =  AOY  -  MJCO 

*  IN  THE  Z  DIRECTION 


MATA(6,6)  =   1.0D0 
hkTAle,!)    =  RBG1(2) 
MATA(6,8)  =  -RBG1(1) 

MATB (6)    =  AOZ  -  MKCO 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 

MATA(7,2' 
MATA(7,3' 
MATA (7,7' 
MATA (7,8' 
MATA(7,9 
MATA(7,li' 
MATA(7,12)  = 

MATB (7)     =   T1X  -  TOX 

MATA (8,1)   =  -RBG1(3) 
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RBGK 

'3) 

-RBGK 

2) 

-IXXT< 

1) 

IXYTI 

l) 

IXZTI 

l) 

-RAG1I 

3) 

RAG1I 

!2) 

MATA(8,3' 

MATA(8,7' 

MATA^^ 

MATA(8,9 

MATA(8,10) 

MATA(8,12) 

MATB(8) 

MATAI 
MATAI 
MATAI 
MATAI 
MATAI 
MATA I 
MATAI 


=  RBG1(1 
=  IXYT(1 
=  -IYYT(1 
=  IYZT(1 
=  RAG1(3' 
=  -RAGl(l! 

=   T1Y  -  TOY 


9,1, 
9,7, 

9,9 

9,10) 

9,11) 


RBG1 
■RBG1 

IXZTi 

IYZTi 

■IZZTi 

■RAG1 

RAG1 


+  IXZTi 

+  IYZTi 
-  IZZTi 


+  IXZTi 
+  IYZTi 

-  IZZTi 


=   T1Z  -  TOZ 


MATB(9) 

*  LINK  TWO 

*  SUM  OF  FORCES  IN  X  DIRECTION 

MATA(10,10)  =  1.0D0 
MATA(10,13)  =  MASS2 
MATA(10,19)  =  -1.0D0 

MATB(IO)     =   0.0D0 

*  SUM  OF  FORCES  IN  THE  Y  DIRECTION 

MATA(11,11)  =  1.0D0 
MATA(ll,14)  =  MASS2 
MATA(11,20)  =  -1.0D0 

MATB(ll)     =   0.0D0 

*  SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA(12,12)  =  1.0D0 
MATA(12,15)  =  MASS2 
MATA(12,21)  =  -1.0D0 

*  SUM  OF  FORCES  LINK  TWO  EQUAL 

MATB(12)     =  -WG2 

*  EQUATIONS  AT  JOINT  ONE 

*  IN  THE  X  DIRECTION 


MATA(13,4) 
MATA(13,8) 
MATA(13,9) 
MATA (13, 13' 
MATA (13, 17, 
MATA(13,18, 


-1.0D0 

-RAG1(3 

RAG1(2 

1.0D0 

RBG2 ( 3 

-RBG2(2 


MATB(13)     =  MIC1  -  MIC2 
IN  THE  Y  DIRECTION 


MATA(14,5) 

MATA(14,7) 

MATA(14,9) 

MATA(14,14' 

MATA(14,16, 

MATA(14,18, 


=  -1.0D0 
=   RAG1(3) 
=  -RAG1(1) 
=   1.0D0 
=  -RBG2(3) 
=   RBG2(1) 


MATB(14)    =  MJC1  -  MJC2 
IN  THE  Z  DIRECTION 


MATA (15,6) 

MATA(15,7) 

MATA(15,8) 

MATA(15,15' 

MATA( 15,16' 

MATA(15,17' 


-1.0D0 

-RAG1(2 

RAG1(1 

1.0D0 

RBG2(2 

-RBG2(1 


I4ATB(15)     =   MKC1  -  MKC2 
SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X,Y,Z  DIRECTIONS 
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* 
* 
* 

* 


MATA(16,11 

MATA(16,12' 
MATA(16,16' 
MATA(16,17' 
MATA(16,18' 
MATA(16,20' 
MATA(16,21 

MATB(16) 


RBG2 
■RBG2< 
■IXXTi 

IXYTi 

IXZTi 
■RAG2i 

RAG2 


=   (-T1X  +  T2X)  *  DCOS(RLRZl) 


MATAl 

17, 

,10) 

MATAl 

17, 

,12 

MATAl 

17, 

,16) 

MATAl 

17, 

,17) 

MATAl 

17, 

,18) 

MATAl 

1?. 

,19 

MATAl 

17, 

,21) 

■RBG2< 
RBG2i 
IXYTl 

■IYYTl 
IYZTl 
RAG2< 

■RAG2' 


MATB(17) 

MATA(18,9) 

MATA(18,18) 

MATB(18) 

MATA(18,10' 

MATAUS,!!' 

MATA(18,16' 

MATA(18,17 

MATA(18,18' 

MATA(18,19 

MATA(18,20 


(-  T1Y  +  T2Y)  *  DSIN(RLRZl) 

■1.0D0 
1.0D0 
0.0D0 


RBG2I 

[2) 

-RBG2I 

1) 

IXZTI 

2) 

IYZTl 

2) 

-IZZTI 

2) 

-RAG2I 

2) 

RAG2I 

!l) 

IXZTi 
IYZTl 
IZZTi 


-  T1Z  +  T2Z 


l.ODO 
MAS  S3 


l.ODO 
MASS3 


MATB(18)  = 

LINK  THREE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 

MATA(19,19)  = 
MATA(19,22)  = 

MATB(19)     =   0.0D0 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 

MATA(20,20)  =   l.ODO 
MATA(20,23)  =  MASS3 

MATB(20)    =  0.0D0 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA(21,21)  = 
MATA(21,24)  = 

MATB(21)  =  -WG3 

EQUATIONS  AT  JOINT  TWO 

IN  THE  X  DIRECTION 

MATA(22,13)  =  -l.ODO 

MATA(22,17)  =  -RAG2(3) 

MATA(22,18)  =  RAG2(2) 

MATA(22,22)  =  l.ODO 

MATA(22,26)  =  RBG3(3) 

MATA(22,27)  =  -RBG3(2) 

MATB(22)    =  MIC3  -  MIC4 

IN  THE  Y  DIRECTION 

MATA(23,14; 
MATA(23,16' 
MATA^,^' 
MATA(23,23' 
MATA(23,25' 
MATA (23,27; 

MATB(23) 


■l.ODO 

RAG2(3) 
■RAG2(1) 

l.ODO 
•RBG3(3' 

RBG3(i; 

MJC3  -  MJC4 
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IN  THE  Z  DIRECTION 

MATA(24,15" 
MATA(24,16' 
MATA(24,17' 
MATA(24,24' 
MATA(24,25' 
MATA(24,26; 

MATB(24) 


-l.ODO 

-RAG2(2) 
RAG2 ( 1 ) 

l.ODO 

RBG3(2) 

-RBG3(1) 

MKC3  -  MKC4 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 
MATA(25,20)  =  RBG3(3: 


X 

* 
* 

* 

*18 
*118 

*81 
*181 


MATA(25,21 
MATA(25,25 
MATA(25,26' 
MATA(25,27 


=  -RBG3< 
=  -IXKTi 
=  IXYTi 
=   IXZTi 


MATB(25)  =  -T2X  *  DCOS(RLRZl) 

MATA(26,19)  =  -RBG3(3 

MATA(26/21)  =   RBG3(1 

MATA(26,25)  =   IXYT(3 

MATA(26,26)  =  -IYYT(3' 

MATA(26,27)  =   IYZT(3' 


MATB(26) 

MATA(27,9) 

MATA(27.27) 

MATB(27) 

MATA(27,19' 
MATA(27,20' 
MATA(27,25' 
MATA(27,26' 
MATA(27,27' 


■T2Y  *   DSIN(RLRZl) 

■l.ODO 
l.ODO 
0.0D0 


RBG3' 

-RBG3' 

IXZT' 

IYZTi 

-IZZTi 


MATB(27)    =  -  T2Z 

GO  TO  1112 

INITIALIZE  MATRIX  ACCORDING  TO  CONSTRAINTS 

CONSTRAINTS  GROUP  1  WHEN  ONLY  LINK  THREE  IS  IN  MOTION 

DO  118  I  =  1,18 
DO  18  J  =  1,27 
MATA(I,J)  =  0.0 
MATA(I.I)  =1.0 
MATB(I)    =0.0 
CONTINUE 
CONTINUE 

DO  181  I  =  19,27 
DO  81  J  =  1,18 

MATA(I,J)  =  0.0 
CONTINUE 
CONTINUE 
GO  TO  1111 

CONSTRAINTS  GROUP  2  WHEN  LINK  TWO  AND  THREE  ARE  'IN  MOTION 

DO  19  I  =  1,9 
DO  191  J  =  1,27 
MATA(I,J)  =  0.0D0 
MATA(I.I)  =  1.0  DO 
MATB(I)    =  0.0D0 

mATA(17,J)  =  0.0D0 

MATA(18.J)  =  0.0D0 

MATB(17)  =  0.0D0 

MATB(18)  =  0.D0 

MATA(J,17)  =  0.0D0 
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* 
* 

* 

*191 

*19 
* 

* 

* 

*92 

*91 
** 


* 
* 

* 

* 
* 
* 

* 
*78 


MATA(J,18)  =  O.ODO 


mATA(17,17)  =  1. 
MATA(18,18)  =  1. 


ODO 
ODO 


CONTINUE 
CONTINUE 

DO  91  I  =  10,27 

DO  92  J  =  1,9 
MATA(I,J)  =  0.0 

CONTINUE 
CONTINUE 
GO  TO  1111 

CONSTRAINTS  GROUP  3  WHEN  THREE  OF  THE  LINKS  ARE  IN  MOTION 


DO  78  J  =  1,27 
MATA(7,j; 
MATA(8,J' 
MATA(J,7 
MATACJ.S' 
MATE-  ( 7 
MATB(8' 


MATA< 

[17, J) 

MATAl 

18, J) 

MATA< 

J<17) 

MATAl 

J,  18) 

MATBI 

17) 
18 

MATB< 

26, J' 
'27, J' 
J, 26' 
J, 27 
26) 
27) 


MATA 
MATA 
MATA 
MATA 
MATB 
MATB 

MATA 
MATA 
MATA 
MATA 
MATA 
MATA 

CONTINUE 


7,7) 

8,i 

3 

17 

,17) 

18 

,18 

26 

,26 

(27 

,27 

O.ODO 
O.ODO 
O.ODO 
O.ODO 
O.ODO 
O.ODO 

O.ODO 
O.ODO 
O.ODO 
O.ODO 
O.ODO 
O.ODO 

O.ODO 
O.ODO 
O.ODO 
O.ODO 
O.ODO 
O.ODO 

=  1.0D0 
=  1.0D0 

■  1.0D0 
:  1.0D0 

■  1.0D0 
'  1.0D0 


*   CONSTRAINT  GROUP  4  THE  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIR, 


1112 

DO  48  I  =  4,8 

DO  481  J  =  1,27 

MATA(I,J)  =  O.ODO 
MATA(I.I)  =  1.0  DO 
MATB(I)    =  O.ODO 

481 

CONTINUE 

48 

CONTINUE 

DO  84  I  =  9,27 

DO  841  J  =  4,8 

MATA(I,J)  =  0.0 

841 

CONTINUE 

84 

CONTINUE 

*  CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

1111   CALL  LEQT2F (MATA, M,N,IA, MATB, IDGT,WKAREA, IER) 

*  IF  (IER  .NE.  0)  CALL  ENDJOB 


FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUES ,WX,WY,WZ 
LINK  ONE 

AX1       =  MATB (4) 
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606 
605 


VELX1  =  INTGRL(0.,AX1) 

LCOGX1  =  INTGRL(X1,VELX1) 

LCOGX(l)  =  LCOGX1 

AY1  =  MATB(5) 

VELY1  =  INTGRL(0. ,AY1) 

LCOGY1  =  INTGRL(Y1,VELY1) 

LCOGY(l)  =  LCOGY1 

AZ1  =  MATB(6) 

VELZ1  =  INTGRL(0. ,AZ1) 

LCOGZ1  =  INTGRL(Z1,VELZ1) 

LCOGZ(l)  =  LCOGZ1 

WD1X  =  MATB(7) 

W1X  =  INTGRL(0.,WD1X) 

WDX(l)  =  WD1X 

Wl(l)  =  W1X 

WD1Y  =  MATB(8) 

W1Y  =  INTGRL(0. ,WD1Y) 

WDY(l)  =  WD1Y 

Wl(2)  =  W1Y 


=  MATB(9) 

=  INTGRL(0. ,WD1Z) 

=  WD1Z 

=  W1Z 


WD1Z 
W1Z 
WDZ(l) 
Wl(3) 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  LINK  ONE 


MAT1R(1,1)  =  DCOS(RLRZl)  * 

MAT1R(2,1)  =  DCOS(RLRZl)  * 
DSIN(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3,1)  =  DCOS(RLRZl)  * 
DSIN(RLRZl)  *  DSIN(YWRXl) 

MAT1R(1,2)  =  DSIN(RLRZ1)*DC0S(PTRY1) 

MAT1R(2,2)  =  DSIN(RLRZl)  *  DSIN(PTRYl) 
DCOS(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3,2)  =  DSIN(RLRZl)  * 
DCOS(RLRZl)  *  DSIN(YWRXl) 

MAT1R(1,3)  =  -DSIN(PTRYl) 

MAT1R(2,3)  =  DCOS(PTRYl)  * 

MAT1R(3,3)  =  DCOS(PTRYl)  * 


DCOS(PTRYl) 

DSIN(PTRYl)  *  DSIN(YWRXl) 

DSIN(PTRYl)  *  DCOS(YWRXl) 

DSIN(YWRXl) 
DSIN(PTRYl)  *  DCOS(YWRXl) 

DSIN(YWRXl) 
DCOS(YWRXl) 


+  . 


+  .  .  . 


GET  THE  VELOCITIES  OF  LINK  ONE  IN  BODY  FIXED  COORDINATE  SYSTEM 


DO  605  J  =  1,3 
SUM1  =  0.0D0 

DO  606  K  =  1,3 
SUM1  =  SUM1  + 
CONTINUE 

BRATEl(J)  =  SUM1 
CONTINUE 


MAT1R(J,K)  *  W1(K) 


TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  NON-ORTHOGONAL  EULER 
COORDINATE  SYSTEM  FOR  LINK  ONE 

MAT1T(1,1)  =  0.0D0 
MAT1T(2,1)  =  1.0D0 
MAT1T(3,1)    =  0.0D0 

MAT1T(1,2)  =   DCOS(YWRXl) 

MAT1T(2,2)  =   DTAN(PTRYl)  *  DSIN(YWRXl) 

MAT1T(3,2)  =   1.0DO/DCOS(PTRY1)  *  DSIN(YWRXl) 

MAT1T(1,3)  s  -DSIN(YWRXl) 

MAT1T(2,3)  =   DTAN(PTRYl)  *  DCOS(YWRXl) 

MAT1T(3,3)  =   l.D0/DCOS(PTRYl)  *  DCOS(YWRXl) 

GET  THE  VELOCITIES  OF  LINK  ONE  IN  THE  EULER  COORDINATE  SYSTEM 
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MAT1T(J,K)  *  BRATEl(K) 


DO  705  J  =  1,3 
SUM1  =  0.0D0 

DO  706  K  =  1,3 
SUM1  =  SUM1  + 
706         CONTINUE 

RATEl(J)  =  SUM1 
705     CONTINUE 

RATE1X  =  RATEl(l) 
RATE1Y  =  RATE1(2) 
RATE1Z  =  RATE1(3) 

INTEGRATION  OF  THE  VELOCITIES  OF  LINK  ONE  IN  EULER  COOR.  SYSTEM 

YWRX1  =  INTGRL(0.,RATE1X) 
PTRY1  =  INTGRL(0. ,RATE1Y) 
RLRZ1  =  INTGRL(-PI/2.,RATE1Z) 

•  CONVERT  THE  ANGLES  TO  DEGREES 

YAWANX(l)  =  YWRX1  *  RADEG 
PTCANY(l)  =  PTRY1  *  RADEG 
ROLANZ(l)  =  RLRZ1  *  RADEG 

GET  THE  DIRECTION  COSINES  FOR  THE  LINK  ONE 

* 


DRCSY(l)  =  DCOS(RLRZl)  *  DSIN(PTRYl) 
DSIN(RLRZl)  *  DSIN(YWRXl) 

DRCSX(l)  =  DSIN(RLRZl)  *  DSIN(PTRYl) 
DCOS(RLRZl)  *  DSIN(YWRXl) 

DRCSZ(l)  =  DCOS(PTRYl)  *  DCOS(YWRXl) 


DCOS(YWRXl)  +... 
DCOS(YWRXl)  -... 


DRCRAXI 
DRCRAY I 
DRCRAZi 

DRCANXl 
DRCANY< 
DRCANZi 


DACOS(DRCSXi 
DACOS(DRCSYl 
DACOS(DRCSZl 

DACOS(DRCSXl 
DACOS(DRCSYl 
DACOS(DRCSZl 


*  RADEG 

*  RADEG 

*  RADEG 


LINK  TWO 

AX2 
VELX2 
LCOGX2 
LCOGX(2) 

AY2 
VELY2 
LCOGY2 
LCOGY(2) 

AZ2 
VELZ2 
LCOGZ2 
LCOGZ(2) 

WD2X 
W2X 
WDX(2) 
W2(l) 

WD2Y 
W2Y 
WDY(2) 
W2(2) 

WD2Z 
W2Z 
WDZ ( 2 ) 
W2(3) 


MATB(13) 

=  INTGRL(0. ,AX2) 
=  INTGRL(X2,VELX2) 
=  LCOGX2 

=  MATB(14) 
=  INTGRL(0. ,AY2) 
=  INTGRL(Y2,VELY2) 
=  LCOGY2 

=  MATB(15) 
=  INTGRL(0. ,AZ2) 
=  INTGRL(Z2,VELZ2) 
=  LCOGZ2 

=  MATB(16) 

=  INTGRL(0. ,WD2X) 

=  WD2X 

=  W2X 

=  MATB(17) 

=  INTGRL(0. ,WD2Y) 

=  WD2Y 

=  W2Y 

=  MATB(18) 

=  INTGRL(0. ,WD2Z) 

=  WD2Z 

=  W2Z 


TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  LINK  TWO 

MAT2R(1,1)  =  DCOS(RLRZ2)  *   DCOS(PTRY2) 
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608 
607 


708 
707 


MAT2R(2,1)  =  DCOS(RLRZ2)  * 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

MAT2R(3,1)  =  DCOS(RLRZ2)  * 
DSIN(RLRZ2)  *  DSIN(YWRX2) 

MAT2R(1,2)  =  DSIN(RLRZ2)  * 

MAT2R(2,2)  =  DSIN(RLRZ2)  * 
DCOS(RLRZ2)  *  DCOS(YWRX2) 

MAT2R(3,2)  =  DSIN(RLRZ2)  * 
DCOS(RLRZ2)  *  DSIN(YWRX2) 

MAT2R(1,3)  =  -DSIN(PTRY2) 

MAT2R(2,3)  =  DCOS(PTRY2)  * 

MAT2R(3,3)  =  DCOS(PTRY2)  * 

GET  THE  VELOCITIES  OF  LINK  TWO 


DSIN(PTRY2) 

DSIN(PTRY2) 

DC0S(PTRY2) 
DSIN(PTRY2) 

DSIN(PTRY2) 


DSIN(YWRX2)  -... 
DCOS(YWRX2) 


DSIN(YWRX2) 
DCOS(YWRX2) 


+  . 


+  . 


DSIN(YWRX2) 
DCOS(YWRX2) 
IN  BODY  FIXED  COORDINATE  SYSTEM 


DO  607  J  =  1,3 
SUM1  =  0.0D0 

DO  608  K  =  1,3 
SUM1  =  SUM1  + 
CONTINUE 

BRATE2(J)  =  SUM1 
CONTINUE 


MAT2R(J,K)  *  W2(K) 


TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  NON- ORTHOGONAL  EULER 
COORDINATE  SYSTEM  FOR  LINK  TWO 


MAT2T 
MAT2T 
MAT2T 

MAT2T 
MAT2T 
MAT2T 

MAT2T 
MAT2T 
MAT2T 


0.0D0 
1.0D0 
0.0D0 

DC0S(YWRX2) 

DTAN(PTRY2)  *  DSIN(YWRX2) 

1.0D0/DCOS(PTRY2)  *  DSIN(YWRX2) 

-DSIN(YWRX2) 
DTAN(PTRY2)  *  DCOS(YWRX2) 
1.0D0/DCOS(PTRY2)  *  DCOS(YWRX2) 

TWO  IN  THE  EULER  COORDINATE  SYSTEM 


GET  THE  VELOCITIES  OF  LINK 

DO  707  J  =  1,3 
SUM1  =  0.0D0 

DO  708  K  =  1,3 

SUM1  =  SUM1  +  MAT2T(J,K)  *  BRATE2(K) 
CONTINUE 
RATE2(J)  =  SUM1 
CONTINUE 

RATE2X  =  RATE2(1) 
RATE 2 Y  =  RATE2(2) 
RATE2Z  =  RATE2(3) 

INTEGRATION  OF  THE  VELOCITIES  OF  LINK  TWO  IN  EULER  COOR.  SYSTEM 

YWRX2  =  INTGRL(0. ,RATE2X; 
PTRY2  =  INTGRL(0. ,RATE2Y; 
RLRZ2  =  INTGRL(-PI/2. ,RATE2Z) 

CONVERT- THE  ANGLES  TO  DEGREES 


YAWANX 
PTCANY 
ROLANZ 


2)  = 
2)  = 
2)  = 


YWRX2 
PTRY2 
RLRZ2 


RADEG 
RADEG 
RADEG 


GET  THE  DIRECTION  COSINES  FOR  THE  LINK  TWO 

DRCSY(2)  =  DCOS(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2) 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

DRCSX(2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)*DSIN(YWRX2) 
DCOS(RLRZ2)  *  DCOS(YWRX2) 

DRCSZ(2)  =  DCOS(PTRY2)  *  DSIN(YWRX2) 

DRCRAX(2)  =  DACOS(DRCSX(2)) 


+  . 
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610 
609 


DRCRAY(2' 
DRCRAZUi 

DRCANX(2' 
DRCANY(2' 
DRCANZ(2; 

JX1   =   i 

JY1  =  i 
JZ1   =   i 


DACOS(DRCSY(2)) 
DACOS(DRCSZ(2)) 

DACOS(DRCSX(2)) 
DACOS(DRCSY(2)) 
DACOS(DRCSZ(2)) 


*  RADEG 

*  RADEG 

*  RADEG 


[1.(1,1)  +  L(l,2' 
L(l,l)  +  L(l,2( 
;L(1,1)  +  L(l,2 


*  DCOS(DRCRAX(l 

*  DCOS(DRCRAY(l 

*  DCOS(DRCRAZ(l 


LINK  THREE 

AX3 
VELX3 
LCOGX3 
LCOGX(3) 

AY3 
VELY3 
LCOGY3 
LCOGY(3) 

AZ3 
VELZ3 
LCOGZ3 
LCOGZ(3) 

WD3X 
W3X 
WDX ( 3 ) 
W3(l) 

WD3Y 
W3Y 
WDY(3) 
W3(2) 

WD3Z 
W3Z 
WDZ(3) 
W3(3) 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  LINK  THREE 

DCOS(PTRY3) 

DSIN(PTRY3) 


=  MATB(22) 

=  INTGRL(0.,AX3) 
=  INTGRL(X3,VELX3) 
=  LCOGX3 

=  MATE- (23) 

=  INTGRL(0. ,AY3) 

=  INTGRL(Y3,VELY3) 

=  LCOGY3 

=  MATB(24) 

=  INTGRL(0. ,AZ3) 

=  INTGRL(Z3,VELZ3) 

=  LCOGZ3 

=  MATB(25) 

=  INTGRL(0.,WD3X) 

=  WD3X 

=  W3X 

=  MATB(26) 

=  INTGRL(0.,WD3Y) 

=  WD3Y 

=  W3Y 

=  MATB(27) 

=  INTGRL(0. ,WD3Z) 

=  WD3Z 

=  W3Z 


MAT3R(1,1)  =  DC0S(RLRZ3)  * 

MAT3R(2,1)  =  DCOS(RLRZ3)  * 
DSIN(RLRZ3)  *  DCOS(YWRX3) 

MAT3R(3,1)  =  DCOS(RLRZ3)  * 
DSIN(RLRZ3)  *  DSIN(YWRX3) 

MAT3R(1,2)  =  DSIN(RLRZ3)  * 

MAT3R(2,2)  =  DSIN(RLRZ3)  * 
DCOS(RLRZ3)  *  DCOS(YWRX3) 

MAT3R(3,2)  =  DSIN(RLRZ3)  * 
DCOS(RLRZ3)  *  DSIN(YWRX3) 

MAT3R(1,3)  =  -DSIN(PTRY3) 

MAT3R(2,3)  =  DCOS(PTRY3)  * 


DSIN(YWRX3) 

DSIN(PTRY3)  *  DCOS(YWRX3) 

DCOS(PTRY3) 

DSIN(PTRY3)  *  DSIN(YWRX3) 

DSIN(PTRY3)  *  DCOS(YWRX3) 


+  . 


+  . 


DSIN(YWRX3) 
MAT3R(3,3)  =  DCOS(PTRY3)  *DCOS(YWRX3) 
GET  THE  VELOCITIES  OF  LINK  THREE  IN  BODY  FIXED  COORDINATE  SYSTEM 


DO  609  J  =  1,3 
SUM1  =  0.0D0 

DO  610  K  =  1,3 
SUM1  =  SUM1  + 
CONTINUE 

BRATE3(J)  =  SUM1 
CONTINUE 


MAT3R(J,K)  *  W3(K) 


TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  NON-ORTHOGONAL  EULER 
COORDINATE  SYSTEM  FOR  LINK  THREE 
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MAT3T(1,1)  =  O.ODO 
MAT3T(2,1)  =  l.ODO 
MAT3T(3,1)  =  O.ODO 

MAT3T(1,2)  =  DCOS(YWRX3) 

MAT3T(2,2)  =   DTAN(PTRY3)  *  DSIN(YWRX3) 

MAT3T'(3,2)  =   1 .0D0/DCOS(PTRY3)  *  DSIN(YWRX3) 

MAT3T(1,3)  =  -DSIN(YWRX3) 

MAT3T(2,3)  =   DTAN(PTRY3)  *  DCOS(YWRX3) 

MAT3T(3,3)  =   1 .  ODO/DCOS (PTRY3)  *  DCOS(YWRX3) 

GET  THE  VELOCITIES  OF  LINK  THREE  IN  THE  EULER  COORDINATE  SYSTEM 

DO  709  J  =  1,3 
SUM1  =  O.ODO 

DO  710  K  =  1,3 

SUM1  =  SUM1  +  MAT3T(J,K)  *  BRATE3(K) 
710         CONTINUE 

RATE3(J)  =  SUM1 
709      CONTINUE 

RATE3X  =  RATE3(1) 
RATE3Y  =  RATE3(2) 
RATE3Z  =  RATE3(3) 

INTEGRATION  OF  THE  VELOCITIES  OF  LINK  THREE  IN  EULER  COOR.  SYSTEM 

YWRX3  =  INTGRL(0. ,RATE3X) 
PTRY3  =  INTGRL(0. ,RATE3Y) 
RLRZ3  =  INTGRL(-PI/2.,RATE3Z) 

CONVERT  THE  ANGLES  TO  DEGREES 

YAWANX(3)  =  YWRX3  *  RADEG 
PTCANY(3)  =  PTRY3  *  RADEG 
ROLANZ(3)  =  RLRZ3  *  RADEG 

GET  THE  DIRECTION  COSINES  FOR  THE  LINK  THREE 

DRCSY(3)  =  DCOS(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  -... 
DSIN(RLRZ3)  *  DCOS(YWRX3) 

DRCSX(3)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  +... 
DCOS(RLRZ3)  *  DCOS(YWRX3) 

DRCSZ(3)  =  DC0S(PTRY3)  *  DSIN(YWRX3) 


DRCRAX(3)  =  DACOS(DRCSX(3)) 

DRCRAY(3)  =  DACOS(DRCSY(3)) 

DRCRAZ(3)  =  DACOS(DRCSZ(3)) 

DRCANX(3)  =  DACOS(DRCSX(3))  *  RADEG 

DRCANY(3)  =  DACOS(DRCSY(3))  *  RADEG 

DRCANZ  3)  =  DACOS(DRCSZ(3))  *  RADEG 


JX2  =  JX1  + 
JY2  =  JY1  + 
JZ2  =  JZ1  + 

TIPX  =  JX2  + 
TIPY  =  JY2  + 
TIPZ  =  JZ2  + 


*  DCOS(DRCRAX(2) 

*  DCOS(DRCRAY  2) 

*  DCOS(DRCRAZ(2): 

*  DCOS(DRCRAX(3 

*  DCOS(DRCRAY(3 

*  DCOS(DRCRAZ(3 


DYNAMIC 
NOSORT 


INPUT  TORQUE  AT  JOINTS 


TOZ  =   A  *  SIN  1 

'W  *  TIME 

+ 

P 

T1X  =-10  *  SIN  1 

W  *  TIME 

+ 

P 

T2X  =  A  *  SIN  1 

;w  *  TIME 

+ 

P 

END 

STOP 

FORTRAN 
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SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD(VECTA,VECTB,MI ,MJ,MK) 
IMPLICIT  REAL*8  (A-Z) 
DIMENSION  VECTA(3),VECTB(3) 

MI  =  VECTA(2)  *  VECTB(3)  -  VECTA(3)  *  VECTB(2 
MJ  =  VECTA(3)  *  VECTB(l)  -  VECTA(l)  *  VECTB(3 
MK  =  VECTA(l)'  *  VECTB(2)  -  VECTA(2)  *  VECTB(1 

RETURN 

END 
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APPENDIX  C 

THREE  DIMENSIONAL  SIMULATION  PROGRAM 

INVESTIGATION  OF  SINGULAR   CONFIGURATION 

TERMINAL 

METHOD  ADAMS 

PRINT  .05, ERROR, ANG12Z , ANG23Z 

CONTROL  FINTIM  =4.0,  DELMAX  =.1,  DELPRT  =  .05 

SAVE  .05,  ERROR, ANG12X, ANG12Y.ANG12Z,THETAB,THETAD, ANG23X,ANG23Y, . .. 

ANG23Z,IYYT(2) ,IXXT(2) ,IZZT(2) 

GRAPH (DE=TEK6 18)  TIME,ANG12X 

GRAPH (DE=TEK6 18)  TIME,ANG12Y 

GRAPH (DE=TEK618)  TIME,ANG12Z 

GRAPH(DE=TEK618)  TIME,ANG23X 

GRAPH (DE=TEK618)  TIME,ANG23Y 

GRAPH(DE=TEK618)  TIME,ANG23Z 

GRAPH(DE=TEK618)  TIME,THETAB 

GRAPH (DE=TEK618)  TIME,IYYT(2) , IXXT(2) , IZZT(2) 


D     DIMENSION  MATA(27 , 27 ) ,MASS (3 , 2) ,L(3, 2) ,RX(3 , 2) ,RY(3 , 2) ,RZ(3 , 2) 

D     DIMENSION  IXX(3 , 2) , IXZ(3 , 2) , IXY(3 , 2) , IYY(3 ,2) , IYZ(3 ,2) , IZZ(3 , 2) 

D     DIMENSION  MAT1R(3 ,3 ) ,MAT2R(3 , 3) ,MAT3R(3 , 3) 

D     DIMENSION  MAT1T(3 , 3) ,MAT2T(3 , 3) ,MAT3T(3 ,3) 

D      INTEGER  IER , I , J ,M,K,P ,N, IA, IDGT , A 

EXCLUDE  I A , IDGT , IER , I , J , M , K , P , N , A 

ARRAY  MATB(27) ,LCOGX(3),LCOGY(3) ,LCOGZ(3) 

ARRAY  VECTA0(3),VECTB0(3) , VECTA1 (3 ) , VECTB1 (3) , VECTA2 (3) , VECTB2 (3) 

ARRAY  WDX(3) ,WDY(3) ,WDZ(3) ,W1(3) ,W2(3) ,W3(3) ,MATC(27 ) ,DQ(27) 

ARRAY  RATE1 ( 3 ) , RATE2 ( 3 ) , RATE3 ( 3 )  , BRATE1 ( 3 )  , BRATE2 ( 3 ) , BRATE3 ( 3 ) 

ARRAY  RBG1(3),RAG1(3),RBG2(3),RAG2(3),RBG3(3) 

ARRAY  SUMHDX(3) ,SUMHDY(3) ,SUMHDZ(3) ,HDX(2) ,HDY(2) ,HDZ(2) ,WKAREA(850) 

ARRAY  IXXT(3) ,IYYT(3) ,IZZT(3) ,IXYT(3) ,IXZT(3) ,IYZT(3) 

ARRAY  YAWANX ( 3 ) , PTCANY ( 3 ) , ROLANZ ( 3 ) 

ARRAY  DRCANX ( 3 ) , DRCANY ( 3 ) , DRCANZ ( 3 ) 

ARRAY  DRCRAX ( 3 ) , DRCRAY ( 3 ) , DRCRAZ ( 3 ) 

ARRAY  DRCSX ( 3 ) , DRCSY ( 3 ) , DRCSZ ( 3 ) 

D     DATA  MATA/729  *  0.0D0/ 

INITIAL 

*  INPUT  PARAMETER  CONSTANTS 

A  =  3.0D0 

P  =  0.0D0 

W  =  PI  /  2.0D0 

IDGT  =  3 

G=0.0D0 

N=27 

M=l 

IA  =27 

*  INPUT  JOINT  LOCATIONS  IN  METERS 

JXO  =  0.0D0 
JYO  =  0.0D0 
JZO  =  0.0D0 
JX1  =  0.0D0 
JY1  =  0.0D0 
JZ1  =  1.0D0 

*  USE  THE  NEXT  SET  OF  JOINT  TWO  COORDINATES  FOR  CASE  A 

JX2  =  0.0D0 
JY2  =  1.0D0 
JZ2  =  1.0D0 

*  USE  THE  NEXT  SET  OF  JOINT  TWO  COORDINATES  FOR  CASE  B 
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L(l,l 
L(l,2 
L(2,l 
L(2,2 
L  3,1 
L(3,2 


MASSI 

1,1) 

MASSI 

1,2 

MASSI 

2,1 

MASSI 

2,2 

MASSI 

3,1 

MASSI 

3,2 

*  JX2  =  l.ODO 

*  JY2  =  O.ODO 

*  JZ2  =  l.ODO 

*  USE  THE  NEXT  SET  OF  JOINT  TWO  COORDINATES  FOR  CASE  C 
* 

*  JX2  =  O.ODO 

*  JY2  -   O.ODO 

*  JZ2  =  2.0D0 

*  INPUT  DISTANCE  FROM  CENTER  OF  LINK  TO  CENTER  OF  MASS  FOR 

*  EACH  LINK  ENDS 

0.50DO 
0.50D0 
0.50D0 
0.50D0 
0.50D0 
0.50D0 

INPUT  MASS  AT  LINK  ENDS  IN  KILOGRAMS 

=  2.5DO 
=  2.5DO 
=  2.5DO 
=  2.5DO 
=  2.5DO 
=   2.5DO 

*  INPUT  OMEGA  AND  OMEGA  DOT 

DO  30  I  =  1,3 

W1(I)  =  O.ODO 

W2(I)  =  O.ODO 

W3(I)  =  O.ODO 

WDX(I)  =  O.ODO 

WDY(I)  =  O.ODO 

WDZ(I)  =  O.ODO 

30         CONTINUE 

*  INPUT  LOCATION  OF  LINK  CENTERS  OF  GRAVITY 

*  LINK  ONE 

LCOGX(l)  =  O.ODO 
XI  =  LCOGX(l) 
LCOGY(l)  =  O.ODO 
Yl  =  LCOGY(l) 
LCOGZ(l)  =  0.5D0 
Zl   =   LCOGZ(l) 

*  NEXT  SET  FOR  LINK  TWO  AND  THREE  TO  USE   FOR  CASE  A 

LCOGX(2)  =  O.ODO 
X2  =  LCOGX(2) 
LCOGY(2)  =  0.5D0 
Y2  =  LCOGY(2) 
LCOGZ(2)  =  l.ODO 
Z2  =  LCOGZ(2) 
LCOGX(3)  =  O.ODO 
X3  =  LCOGX(3) 
LCOGY(3)  =  1.5D0 
Y3  =  LCOGY(3) 
LCOGZ(3)  =  l.ODO 
Z3   =  LCOGZ(3) 

*  NEXT  SET  FOR  LINK  TWO  AND  THREE  TO  USE   FOR  CASE  B 

*  LCOGX(2)  =  0.5D0 

*  X2      LCOGX(2) 

*  LCOGY(2)  =  O.ODO 

*  Y2      LCOGY(2) 

*  LCOGZ(2)  =  l.ODO 

*  Z2      LCOGZ(2) 

*  LCOGX(3)  =  1.5D0 

*  X3      LCOGX(3) 
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X 


*  LC0GY(3)  =  O.ODO 

*  Y3      LCOGY(3) 

*  LCOGZ(3)  =  l.ODO 

*  Z3      LCOGZ(3) 

*  .      NEXT  SET  FOR  LINK" TWO  AND  THREE  TO  USE   FOR  CASE  C 

*  LCOGX(2)  =  O.ODO 

*  X2      LCOGX(2) 

*  LCOGY(2)  =  O.ODO 

*  Y2      LCOGY(2) 

*  LCOGZ(2)  =  1.5D0 

*  Z2      LCOGZ(2) 

*  LCOGX(3)  =  O.ODO 

*  X3      LCOGX(3) 

*  LCOGY(3)  =  O.ODO 

*  Y3      LCOGY(3) 

*  LCOGZ(3)  =  2.5D0 

*  Z3      LCOGZ(3) 

*  INPUT  MASS  OF  EACH  LINK  IN  KG  AND  COMPUTE  WEIGHTS  IN  NEWTONS 

MASS1  =  5.0D0 
MASS2  =  5.0DO 
MASS3  =  5.0D0 

WG1  =  MASSING 
WG2  =  MASS2*G 
WG3  =  MASS3*G 

*  INPUT  ACCELERATION  OF  JOINT  ZERO 

AOX  =  O.ODO 
AOY  =  O.ODO 
AOZ  =  O.ODO 

*  INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  40  I  =  1,27 
DO  50  J  =  1,27 

MATA(I,J)  =  O.ODO 
DQ(I)  =  O.ODO 
MATC(I)  =  O.ODO 
50  CONTINUE 

40        CONTINUE 

DO  60  I  =  1,27 
MATB(I)  =  O.ODO 
60        CONTINUE 

*  INITIALIZE  THE  INITIAL  VELOCITIES  AND  TRANSFORMATION  MATRICIES 

DO  63  I  =  1,3 
DO  64  J  =  1,3 


RATEl(I) 

- 

O.ODO 

RATE2(I) 

— 

O.ODO 

RATE3(I) 

= 

O.ODO 

BRATEl(I) 

= 

O.ODO 

BRATE2(I 

= 

O.ODO 

BRATE3(IJ 

= 

O.ODO 

MAT1T  (I, 

j; 

= 

O.ODO 

MAT2T   I, 

J 

= 

O.ODO 

MAT3T  (I, 

J 

= 

O.ODO 

MAT1R  (I, 

J 

= 

O.ODO 

MAT2R   I, 

j 

= 

O.ODO 

MAT3R  (I, 

J) 

= 

O.ODO 

CONTINUE 

CONTINUE 

64 
63 


DERIVATIVE 
NOSORT 

CALL  ERRSET  (208,256,-1,1,1) 

LEVELQ  =  0 

CALL  UERSET( LEVELQ, LEVLDQ) 
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INITIALIZE  MATRIX  A  AND  B  TO  ZERO 

DO  70  I  =  1,27 
DO  80  J  =  1,27 

MATA(I,J)  =  0.0D0 
80  CONTINUE 

70        CONTINUE 

DO  90  I  =  1,27 
MATB(I)  =  0.0D0 
DQ(I)  =  0.0D0 
90        CONTINUE 


INPUT  JOINT  EQUATIONS 

JOINT  ZERO   EQUATIONS 

AB  =  AG1  +  (WD1  X  RB/G1)  +  Wl  X  (Wl  X  RB/G1) 

VECTAO(l)  =  WDX(l) 
VECTAO (2)  =  WDY(l) 
VECTA0(3)  =  WDZ(l) 

RBG1(1)  =  JXO  -  LCOGX(l) 
RBG1(2)  =  JYO  -  LCOGY(l) 
RBG1(3)  =  JZO  -  LCOGZ(l) 

CALL  CPROD ( VECTAO , RBG1 , MI AO , MJAO , MKAO ) 

VECTAO(l)  =  Wl(l) 
VECTA0(2)  =  Wl(2) 
VECTAO (3)  =  Wl(3) 

CALL  CPROD (VECTAO , RBG1 , MIBO , MJBO , MKBO ) 

VECTBO(l)  =  MIBO 
VECTB0(2)  =  MJBO 
VECTB0(3)  =  MKBO 

CALL  CPROD(VECTA0,VECTB0,MIC0,MJC0,MKC0) 

JOINT  ONE  EQUATIONS--- 

AA  =  AG1  +  (WD1  X  RA/G1)  +  Wl  X  (Wl  X  RA/G1) 

VECTA1 
VECTA1 
VECTA1 


RAG1 
RAG1 
RAG1 


LCOGXi 
LCOGYi 
LCOGZi 


CALL  CPROD (VECTA1 , RAG1 , MI Al , M JA1 , MKA1 ) 


VECTA1I 

'X< 

1  =  Wll 

;i) 

VECTA1I 

2 

i  =  Wl< 

2) 

VECTAll 

|3] 

i  =  Wl< 

!3) 

CALL  CPROD  (VECTA1,RAG1,MIB1,MJB1,MKB1) 

VECTBl(l)  =  MIB1 
VECTB1(2)  =  MJB1 
VECTB1(3)  =  MKB1 

CALL  CPROD  (VECTA1,VECTB1,MIC1,MJC1,MKC1) 

AB  =  AG2  +  (WD2  X  RB/G2)  +  W2  X  (W2  X  RB/G2) 


VECTA1 
VECTA1 
VECTA1 


RBG2< 
RBG2' 
RBG2 


1)  =  WDX' 

2)  =  WDY 

3)  =  WDZ 

=  JX1  - 
=  JY1  - 
=  JZ1  - 


'2) 
'2) 
\2) 

LCOGXi 
LCOGYi 

LCOGZi 


CALL  CPROD  (VECTA1,RBG2,MIA2,MJA2,MKA2) 
VECTAl(l)  =  W2(l 


VECTA1 


(2)  =  W2(2) 
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VECTA1(3)  =  W2(3) 

CALL  CPROD  (VECTA1,RBG2,MIB2,MJB2,MKB2) 

VECTBl(l)  =  MIB2 
VECTB1(2)  =  MJB2 
VECTB1(3)  =  MKB2 

CALL  CPROD  (VECTA1#VECTB1,MIC2,MJC2,MKC2) 

JOINT  TWO  EQUATIONS 

AA  =  AG2  +  (WD2  X  RA/G2)  +  W2  X  (W2  X  RA/G2) 


VECTA2< 
VECTA2' 
VECTA2< 

RAG2 ( 1 

RAG2(2' 

RAG2(3' 


=  WDX(2) 
=  WDY(2) 
=  WDZ(2) 

JX2  -  LCOGX(2 
JY2  -  LCOGY(2 
LCOGZ(2 


=  JZ2 


CALL  CPROD  (VECTA2,RAG2,MIA3,MJA3,MKA3) 

VECTA2(1)  =  W2(l) 
VECTA2(2)  =  W2(2) 
VECTA2(3)  =  W2(3) 

CALL  CPROD  (VECTA2,RAG2,MIB3,MJB3,MKB3) 

VECTB2(1)  =  MIB3 
VECTB2(2)  =  MJB3 
VECTB2(3)  =  MKB3 

CALL  CPROD ( VECTA2 , VECTB2 , MIC3 , MJC3 , MKC3 ) 

AB  =  AG3  +  (WD3  X  RB/G3)  +  W3  X  (W3  X  RB/G3) 


VECTA2' 
VECTA2i 
VECTA2i 


=  WDXi 
=  WDYi 
=  WDZi 


RBG3(1)  =  JX2  -  LCOGX(3) 
RBG3(2)  =  JY2  -  LCOGY(3) 
RBG3(3)  =  JZ2  -  LCOGZ(3) 

CALL  CPROD  (VECTA2,RBG3,MIA4,MKA4,MKA4) 

VECTA2(1)  =  W3(l) 
VECTA2(2)  =  W3(2) 
VECTA2(3)  =  W3(3) 

CALL  CPROD  (VECTA2,RBG3,MIB4,MJB4,MKB4) 

VECTB2(1)  =  MIB4 
VECTB2(2)  =  MJB4 
VECTB2(3)  =  MKB4 

CALL  CPROD  (VECTA2,VECTB2,MIC4,MJC4,MKC4) 

SUM  OF  MOMENTS  EQUATIONS 

DO  100  I  =  1,3 

COMPUTE  HX,HDOT  X,HY,HDOT  Y,  HZ,HDOT  Z 


rx(i,i; 

I    =    -L< 

I1'1) 

RX(I,2 

l    =      L( 

X'2 

RY(I,1 

I    =   -L< 

1,1 

RY(I,2 

I    =      L( 

I1'2) 

RZ ( 1 , 1 

l    =   -L< 

I1*1! 

RZ ( 1 , 2 

l    =      L( 

1,2) 

DCOS(DRCRAXl 
DCOS(DRCRAX( 
DCOS(DRCRAYi 
DCOS(DRCRAY( 
DCOS(DRCRAZi 
DCOS(DRCRAZi 


IXX(I,1)  =  MASS(I,1 
IXX(I,2)  =  MASS (I, 2 


RY  1,1 
v(RY(I.2) 

IXXT(I)   =  IXX(I,1)  +  IXX(I,2) 

IF  (IXXT(I)  .LE.  .020)  THEN 

IXXT(I)   =  .020 

ELSE 

IXXT(I)  =  IXXT(I) 

END  IF 


*  RY(I,1 

*  RY(I,2 


(RZ(I,1 
(RZ(I,2 


*  RZ(I,1) 
,2) 


RZ(I 


87 


IXY(I,1)  =  MASS(I,1)  *  RX(I,1)  *  RY(I,1) 
1X7(1,2)  =  MASS(I,2)  *  RX(I,2)  *  R7(I,2) 
IXYT(I)   =  1X7(1,1)  +  IXY(I,2) 

IXZ(I,1)  =  MASS(I,1)  *  RZ(I,1)  *  RX(I,1) 
IXZ(I,2)  =  MASS(I,2)  *  RZ(I,2)  *  RX(I,2) 
IXZT(I)   =  IXZ(I,D  +  IXZ(I,2) 

HDX(l)  =  WDX(l)  *  IXX(I,1)-  WDZ(I)  *  IXZ(I,1)  -  WDY(I)  *  1X7(1,1) 
HDX(2)  =  WDX(2)  *  IXX(I,2)-  WDZ(I)  *  IXZ(I,2)  -  WDY(I)  *  IXY(I,2) 

IYY(I,1)  =  MASS(I,1)  *  ((RX(I,1)  *  RX(I,1))  +  (RZ(I,1)  *  RZ(I,1))) 

177(1,2)  =  MASS(I,2)  *  ((RX(I,2)  *  RX(I,2>)  +  (RZ(I,2)  *  RZ(I,2))) 

I77T(I)   =  177(1,1)  +  177(1,2) 

IF  (IYYT(I)  .LE.  .020)  THEN 

IYYT(I)   =  .020 

ELSE 

I77T(I)   =  I77T(I) 

END  IF 

I7Z(I,1)  =  MASS(I,1)  *  R7(I,1)  *  RZ(I,1) 
IYZ(I,2)  =  MASS(I,2)  *  R7(I,2)  *  RZ(I,2) 
I7ZT(I)   =  I7Z(I,1)  +  I7Z(I,2) 

HD7(1)  =  WD7(I)  *  177(1,1)  -  WDX(I)  *  1X7(1,1)  -  WDZ(I)  *  I7Z(I,1) 
HD7(2)  =  WD7(I)  *  177(1,2)  -  WDX(I)  *  1X7(1,2)  -  WDZ(I)  *  I7Z(I,2) 

IZZ  1,1)  =  MASS(I,1)  *   (RX(I,1)  *  RX(I,1))  +  (R7(I,1)  *  R7(I,1)) 

IZZ(I,2)  =  MASS(I,2)  *  ((RX(I,2)  *  RX(I,2))  +  (R7(I,2)  *  R7(I,2))) 

IZZT(I)  =  IZZ(I,1)  +  IZZ(I,2) 

IF  (IZZT(I)  .LE.  .020)  THEN 

IZZT(I)  =  .020 

ELSE 

IZZT(I)  =  IZZT(I) 

END  IF 

HDZ(l)  =  WDZ(I)  *  IZZ(I,1)  .-  WDX(I)  *  IXZ(I,1)  -  WD7(I)  *  I7Z(I,1) 
HDZ(2)  =  WDZ(I)  *  IZZ(I,2)  -  WDX(I)  *  IXZ(I,2)  -  WD7(I)  *  I7Z(I,2) 

SUMHDX(I)  =  HDX(l)  +  HDX(2) 
SUMHDY(I)  =  HDY(l)  +  HD7(2) 
SUMHDZ(I)  =  HDZ(l)  +  HDZ(2) 


100 

CONTINUE 

* 

ENTER  CONSTANTS  INTO  MATRIX  A 

* 

LINK  ONE 

* 

SUM  OF  FORCES  IN  THE  X  DIRECTION 

MATA(1,1)   =   l.ODO 
MATA(1,4)   =   MASSl 
MATA(1,10)  =  -l.ODO 

* 

SUM  OF  FORCES  IN  7  DIRECTION   ' 

MATA(2,2)  =  l.ODO 
MATA(2,5)  =  MASSl 
MATA(2,11)  =  -l.ODO 

*  SUM  OF  FORCES  IN  Z  DIRECTION 

MATA(3,3)  =  l.ODO 
MATA(3,6)  =  MASSl 
MATA(3,12)  =  -l.ODO 

*  EQUATIONS  AT  JOINT  ZERO 

*  IN  THE  X  DIRECTION 

MATA(4,4)  =   l.ODO 
MATA(4,8)  =   RBG1(3) 
MATA(4,9)  =  -RBG1(2) 

*  IN  THE  Y  DIRECTION 

MATA(5,5)  =   l.ODO 
MATA(5,7)  =  -RBG1(3) 
MATA(5,9)  =   RBG1(1) 
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IN  THE  Z  DIRECTION 


MATA(6,6' 
MATA(6,7' 
MATA(6,8' 


l.ODO 
RBG1(2) 
■RBG1(1) 


SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  ONE  IN  THE  X,Y,Z  DIRECTIONS 


MATA 
MATA 
MATA 
MATA 
MATA 
MATA 
MATA 


MATA (8 
MATA (8 
MATA (8 
MATA ( 8 
MATA (8 
MATA (8 
MATA (8 


11)  = 

12)  = 


10)  = 
12)  =  - 


10)  = 

11)  = 


RBG1 
■RBG1 
■IXXT 

IXYT 

IXZT 
■RAG1 

RAG1 

■RBG1 
RBG1 
IXYT  i 

■IYYTi 

IYZTi 

RAG1 

RAG1 

RBG1 
■RBGli 

IXZT  i 

IYZTi 
■IZZTi 
■RAG1- 

RAG1 


IXZT 
IYZT 
IZZT 


MATA  I 
MATA  I 
MATA  i 
MATA  i 
MATA  i 
MATA  l 
MATA< 

LINK  TWO 

SUM  OF  FORCES  IN  X  DIRECTION 

MATA(10/10)  =  l.ODO 
MATA(10,13)  =  MASS2 
MATA(10,19)  =  -l.ODO 

SUM  OF  FORCES  IN  THE  Y  DIRECTION 


IXZT  I 
IYZTi 

IZZT  i 


MATA(ll,li; 

MATA(11,14 

MATA(11,20, 


l.ODO 
MASS  2 
■l.ODO 


SUM  OF  FORCES  IN  THE  Z  DIRECTION 

MATA(12,12)  =  l.ODO 
MATA(12,15)  =  MASS2 
MATA(12,21)  =  -l.ODO 

EQUATIONS  AT  JOINT  ONE 

IN  THE  X  DIRECTION 

MATA(13,4)  =  -l.ODO 

MATA(13,8)  =  -RAG1(3 

MATA(13,9)  =  RAG1(2 

MATA(13,13)  =  l.ODO 

MATA(13,17)  =  RBG2(3 

MATA(13,18)  =  -RBG2(2 

IN  THE  Y  DIRECTION 

-l.ODO 
RAG1(3) 

-RAGl(l) 
l.ODO 

-RBG2(3 
RBG2(1 

IN  THE  Z  DIRECTION 


MATA! 

14 

5) 

MATAI 

14 

7) 

MATAI 

14 

9) 

MATAI 

14 

14) 

MATAI 

14 

16 

MATA 

14 

18) 

MATA 
MATA 
MATA 
MATA 
MATA 


15,6 
15,7 

15,8, 


15.15)  = 

15.16)  = 


=  -l.ODO 
=  -RAG1(2) 
=   RAG1(1) 
l.ODO 
RBG2(2) 
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MATA(15,17)  =  -RBG2(1) 
SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  TWO  IN  THE  X,Y,Z  DIRECTIONS 


MATA(16 
MATA(16 
MATA (16 
MATA(16 
MATA (16 
MATA(16 
MATA (16 


MATA 
MATA 
MATA 
MATA 
MATA 
MATA 
MATA 

MATA 
MATA 
MATA 
MATA 
MATA 
MATA 
MATA 


17 
17 
17 
17 
17 
17 
17 

18 
18 
18 
18 
18 
18 
18 


11' 
12" 
16' 
17' 
18' 
20' 

21; 

10 
12 

16' 
17' 

is; 

19 

21; 

10 

11 

16 

17' 

18' 

19 

20' 


RBG2 
-RBG2' 
-IXXTi 

IXYTi 

IXZTi 
-RAG2' 

RAG2' 

-RBG2 
RBG2i 
IXYTi 

-IYYTi 
IYZTi 
RAG2< 

-RAG2 

RBG2> 
-RBG2i 

IXZTi 

IYZTi 
-IZZTi 
-RAG2i 

RAG2< 


IXZT(3" 
IYZT(3' 
IZZT(3' 


LINK  THREE 

SUM  OF  FORCES  IN  THE  X  DIRECTION 
MATA(19,19)  =   1.0D0 


MATA(19',22)  =  MASS3 


SUM  OF  FORCES  IN  THE  Y  DIRECTION 

MATA(20,20)  =   1.0D0 
MATA(20,23)  =  MASS3 

SUM  OF  FORCES  IN  THE  Z  DIRECTION 


MATA(21,21)  = 
MATA(21,24)  = 


1.0D0 

MAS  S3 


EQUATIONS  AT  JOINT  TWO 

IN  THE  X  DIRECTION 

MATA(22;13)  =  -1.0D0 

MATA(22,17 

MATA(22,18 

MATA (22 ,22' 

MATA (22, 26' 

MATA(22,27; 

IN  THE  Y  DIRECTION 

MATA(23,14' 
MATA (23, 16 
MATA (23, 18' 
MATA (23, 23' 
MATA(23,25' 
MATA(23,27, 

IN  THE  Z  DIRECTION 


-RAG2(3) 
RAG2(2) 
1.0D0 
RBG3(3) 

-RBG3(2) 


-1.0D0 
RAG2(3) 

-RAG2(1) 
1.0D0 

-RBG3(3) 
RBG3(1) 


=  -1.0D0 

=  -RAG2(2) 

=  RAG2(1) 

=  1.0D0 

=  RBG3(2) 

=  -RBG3(1) 

SUM  OF  MOMENTS  EQUATIONS  FOR  LINK  THREE  IN  THE  X,Y,Z  DIRECTIONS 

MATA (25, 20)  =  RBG3(3) 

MATA(25,21)  =  -RBG3(2) 

MATA(25,25)  =  -IXXT(3) 


MATAI 

;24 

15) 

MATAI 

24 

16 

MATAI 

24 

17 

MATAI 

24 

24 

MATAI 

24 

25 

MATAI 

J24 

26) 
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x 
x 

*18 

*118 

* 

x 
* 

* 

*81 
*181 


* 

x 
x 

x 

* 

X 
X 

*191 
*19 

* 

*92 
*91 

* 

X 
X 
X 
X 
X 


MATA(25,26)  =   IXYT(3 
MATA(25,27)  =   IXZT(3 

MATA(26,19)  =  -RBG3(3 

MATA(26,21)  =  RBG3(1 

MATA(26,25)  =  IXYT(3' 

MATA(26,26)  =  -IYYT(3' 

MATA(26,27)  =  IYZT(3; 

MATA(27,19)  =  RBG3(2' 

MATA(27,20)  =  -RBG3(l' 

MATA(27,25)  =   IXZT  3' 

MATA(27,26)  =   IYZTC3' 

MATA(27,27)  =  -IZZT(3! 

GO  TO  1112 

INITIALIZE  MATRIX  ACCORDING  TO  CONSTRAINT 

CONSTRAINTS  GROUP  1  WHEN  ONLY  LINK  THREE  IS  IN  MOTION 

DO  118  I  =  1,18 
DO  18  J  =  1,27 
MATA(I,J)  =  0. 
MATA(I.I)  =  1. 
MATB ( I ) 
CONTINUE 
CONTINUE 


0 
0 
=  0.0 


DO  181  I  =  19,27 
DO  81  J  =  1,18 

MATA(I,J)  =  0.0 
CONTINUE 
CONTINUE 
GO  TO  11-11 

CONSTRAINTS  GROUP  2  WHEN  LINK  TWO  AND  THREE  ARE  IN  MOTION 


DO  19  I  =  1,9 
DO  191  J  =  1,27 
MATA(I 
MATA ( I 


,J)  =  0. 
.1)  =  1 


0D0 
0  DO 


MATB(lj    =  0!0D0 

MATA (17, J)  =  0.0D0 

MATA(18.J)  =  0.0D0 

MATB(17)  =  0.0D0 

MATB(18)  =  0.D0 

MATA(J,17)    =   0.0D0 
MATA(J,18)    =   0.0D0 

MATA(17,17)    =    1.0D0 
MATA(18,18)    =   1.0D0 

CONTINUE 
CONTINUE 

DO  91  I  =  10,27 
DO  92  J  =  1,9 
MATA(I,J)  =0.0 

CONTINUE 
CONTINUE 
GO  TO  1111 


CONSTRAINTS  GROUP 
DO  78  J  =  1,27 
MATA ( 7  ,  J 


3  WHEN  THREE  OF  THE  LINKS  ARE  IN  MOTION 


MATA ( 8 
MATA (J 
MATA (J 
MATB ( 7 
MATB (8 


0.0D0 
0.0D0 
0.0D0 
0.0D0 
0.0D0 
0.0D0 


91 


* 

MATA( 

;i7,j) 

=  0.0D0 

* 

MATAI 

18,  J) 

=  0.0D0 

* 

MATAI 

J, 17) 

=  0.0D0 

* 

MATAI 

J, 18) 

=  0.0D0 

* 

MATB  I 

lis) 

=  0.0D0 

* 

MATB  I 

=  0.0D0 

* 

MATAI 

[26, J) 

=  0.0D0 

* 

MATAI 

27, J 

=  O.ODO 

* 

MATAI 

J/26) 

=  O.ODO 

* 

MATA< 

J,  27) 

=  O.ODO 

* 

MATB  ( 

[21) 

=  O.ODO 

* 

MATB 1 

=  O.ODO 

* 

MATAI 

[1,1) 
8,8) 

=  l.ODO 

* 

MATAI 

=  l.ODO 

* 

MATAI 

17,17) 

i  =  l.ODO 

* 

MATAI 

18,18 

1  =  l.ODO 

* 

MATAI 

26,26 

i  =  l.ODO 

* 

MATAI 

!27,27) 

I  =  l.ODO 

*78 

CONTIf 

IUE 

*  CONSTRAINT  GROUP  4  THE  FIRST  LINK  IS  CONSTRAINED  TO  ROTATE  IN  Z  DIR. 

1112    DO  48  I  =  4,8 

DO  481  J  =  1,27 

MATA(I,J)  =  O.ODO 

MATA(I.I)  =  1.0  DO 

MATB(I)    =  O.ODO 
481      CONTINUE 
48      CONTINUE 

DO  84  I  =  9,27 
DO  841  J  =  4,8 
MATA(I,J)  =  0.0 
841       CONTINUE 
84      CONTINUE 

*  USE  THE  FOLLOWING  SET  OF  INFORMATION  WHEN  THE  ANGULAR  VELOCITY  IS 

*  IN  X  DIRECTION  REGARDLESS  OF  THE  INITIAL  CONFIGURATION 

*  ENTER  THE  THEORITICAL  VALUES  ASSUMING  THE  LINK  TWO  AND  THREE  ARE 

*  IN  PLANAR  MOTION  AND  ANGULAR  VELOCITY  IS  IN  THE  X  DIRECTION 

*  LINK  TWO 

*  THEORITICAL  ANGULAR  VELOCITIES  (APPLIED  IN  THE  X  DIRECTION) 

MATB (18)  =  O.ODO 
MATB (17)  =  O.ODO 
MATB(16)  =  -((PI**3)  /  8.0D0)  *  DSIN(W  *  TIME) 

THDDOT  =  MATB (16) 

THTDOT  =  INTGRL((PI**2)/4., THDDOT) 

THETRB  =  INTGRL(0. , THTDOT) 

THETAB  =  THETRB  *  RADEG 

*  LINEAR  VELOCITIES 

MATB(15)  =  -(THDDOT  *  RBG2(2))  +  (THTDOT  **  2)  *  RBG2(3 
MATB(14)  =  (THDDOT  *  RBG2(3))  +  (THTDOT  **  2)  *  RBG2(2 
MATB(13)  =   O.ODO 

*  LINK  THREE 

*  ANGULAR  VELOCITIES 

MATB (27)  =  O.ODO 

MATB (26)  =  O.ODO 

MATB (25)  =  O.ODO 
* 

*  LINEAR  VELOCITIES 

MATB(24)  =  MATB(15)+(THDDOT*RAG2(2 
MATB (23)  =  MATB(1< 
MATB (22)  =  MATB(i;  . 

*  END  OF  THE  INFORMATION  FOR  X  DIRECTION 


(THDDOT*RAG2 ( 2 ) ) - (THTDOT**2 ) * ( RAG2 ( 3 ) ) 
(THDDOT*RAG2(3))-(THTDOT**2)*(RAG2(2)) 
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* 
* 

* 
* 
* 


USE  THE  FOLLOWING  SET  OF  INFORMATION  WHEN  THE  ANGULAR  VELOCITY  IS 
IN  THE  Y  DIRECTION  REGARDLESS  OF  THE  INITIAL  CONFIGURATION 

ENTER  THE  THEORITICAL  VALUES  ASSUMING  THE  LINK  TWO  AND  THREE  ARE 
IN  PLANAR  MOTION  AND  ANGULAR  VELOCITY  IS  IN  THE  Y  DIRECTION 

LINK  TWO 

THEORITICAL  ANGULAR  VELOCITIES (APPLIED  IN  THE  Y  DIRECTION  ) 


0.0D0 

( (-PI**3)/8)*SIN(W*TIME) 

0.0D0 

MATB(17) 

INTGRL((PI**2)/4. ,THDDOT) 

INTGRL(0,INTGRL(PI**2/4. ,THDDOT)) 


=   THETRB  *  RADEG 


RBG2(1)) 
RBG2(3)) 


(THTDOT  **  2)  *  RBG2(3) 
(THTDOT  **  2)  *  RBG2(1) 


MATB(18 
MATE (17 
MATB (16 
THDDOT 
THTDOT 
THETRB 
THETAB 
LINEAR  VELOCITIES 

MATB(15)  =   (THDDOT 
MATB (14)  =   0.0DO 
MATB (13)  =  -(THDDOT 

LINK  THREE 
ANGULAR  VELOCITIES 

MATB(27)  =  0.0D0 
MATB (26)  =  O.ODO 
MATB(25)  =  O.ODO 

LINEAR  VELOCITIES 

MATB(24)  =  MATB(15)-(THDDOT*RAG2(l))-(THTDOT**2)*(RAG2(3)) 

MATB (23)  =  MATB (14) 

MATB(22)  =  MATB(13)+(THDDOT*RAG2(3))-(THTDOT**2)*(RAG2(l)) 

END  OF  THE  INFORMATION  FOR  THE  Y  DIRECTION 


USE  THE  FOLLOWING  SET  OF  INFORMATION  WHEN  THE  ANGULAR  VELOCITY  IS 
IN  THE  Z  DIRECTION  REGARDLESS  OF  THE  INITIAL  CONFIGURATION 

ENTER  THE  THEORITICAL  VALUES  ASSUMING  THE  LINK  TWO  AND  THREE  ARE 
IN  PLANAR  MOTION  AND  ANGULAR  VELOCITY  IS  IN  THE  Z  DIRECTION 

LINK  TWO 

THEORITICAL  ANGULAR  VELOCITIES (APPLIED  IN  THE  Z  DIRECTION  ) 

MATB(16)  =  O.ODO 

MATB (17)  =  O.ODO 

MATB(18)  =  -((PI**3)  /  8.0D0)  *  DSIN(W  *  TIME) 

THDDOT  '  =  MATB(18) 

THTDOT  =  INTGRL((PI**2)/4. , THDDOT) 

THETRB  =  INTGRL(0. , THTDOT) 

THETAB  =  THETRB  *  RADEG 

LINEAR  VELOCITIES 

MATB(14)  =  -(THDDOT  *  RBG2(1))  + 
=   (THDDOT  *  RBG2(2))  + 


MATB 
MATB 


13 
15)  = 


(THTDOT  ** 
(THTDOT  ** 


O.ODO 


)  *  RBG2(2) 
)  *  RBG2(1) 


LINK  THREE 
ANGULAR  VELOCITIES 

MATB (27)  =  O.ODO 
MATB(26)  =  O.ODO 
MATB(25)  =  O.ODO 

LINEAR  VELOCITIES 

MATB (24)  =  MATB (15' 


(THTDOT**2)*(RAG2(2 
(THTDOT**2)*(RAG2(l 


MATB (23)  =  MATB(14)+(THDD0T*RAG2(1)) 
MATB (22)  =  MATB(l3)-(THDDOT*RAG2(2))' 

END  OF  THE  INFORMATION  FOR  THE  Z  DIRECTION 

NEXT  SET  OF  STATEMENTS  ARE  COMMON  IN  ANY  PLANAR  MOTION  AND  THEY  ARE 
IN  YHE  CODE  IN  EVERY  CASE.  THESE  TERMS  ARE  ACCELERATION  OF  THE  LINK 
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ONE  AND  FORCES  AT  EACH  JOINT 
LINK  ONE  LINEAR  AND  ANGULAR  ACCELERATIONS 


MATB(4' 
MATB(5, 
MATB (6' 
MATB (7' 
MATB(8' 
MATB ( 9 ' 


FORCES 
JOINT  TWO 


MATB(21' 
MATB (20' 
MATB (19; 

JOINT  ONE 

MATB (12' 
MATB (11' 
MATB ( 10 , 

JOINT  ZERO 

MATB (3) 
MATB ( 2 ) 
MATB ( 1 ) 


0.0D0 
0.0D0 
0.0D0 
0.0D0 
0.0DO 
0.0D0 


-MAS S3 
-MASS3 
-MASS3 


MATB (21' 
MATB (20' 
MATB (19' 


MATB (24' 
MATB (23 
MATB (22 


MASS  2 
MASS  2 
MASS2 


MAS  SI 
MAS  SI 
MAS  SI 


-  WG3 


MATB (15)  -WG2 
MATB (14) 
MATB (13) 


MATB  i 
MATB  i 
MATB  i 


-WG1 


555 
505 

506 


MATA(J,K)  *  MATB(K) 


=  MATB (12 
=  MATB(11' 
=  MATB(10; 

END  OF  THE  INFORMATION 

MULTIPLY  MATA  AND  MATB 

DO  505  J  =  1,27 
SUM1  =0.0 

DO  555  K  =  1,27 
SUM1  =  SUM1  + 
CONTINUE 
DQ(J)  =  SUM1 
CONTINUE 

DO  506  I  =1,27 

MATC(I)  =  DQ(I) 
CONTINUE 

CALL  EQUATION  SOLVER  PROGRAM  FROM  IMSL 

CALL  LEQT2F(MATA,M,N,IA,DQ,IDGT,WKAREA,IER) 

IF  (IER  .NE.  0)  CALL  END JOB 

FIND  LCOGX,LCOGY,LCOGZ,THETA  VALUES ,WX,WY,WZ 

LINK  ONE 


AX1 
VELX1 
LCOGX1 
LCOGX(l) 

AY1 
VELY1 
LCOGY1 
LCOGY(l) 

AZ1 
VELZ1 
LCOGZ1 
LCOGZ(l) 

WD1X 
W1X 
WDX(l) 
Wl(l) 

WD1Y 
W1Y 
WDY(l) 
Wl(2) 


=  DQ(4) 

=  INTGRL(0. ,AX1). 
=  INTGRL(X1,VELX1) 
=  LCOGX1 

=  DQ(5) 

=  INTGRL(0. ,AY1) 
=  INTGRL(Y1,VELY1) 
=  LCOGY1 

=  DQ(6) 

=  INTGRL(0. ,AZ1) 
=  INTGRL(Z1,VELZ1) 
=  LCOGZ1 


=  DQ(7) 

=  INT 


ITGRL(0. ,WD1X) 
=  WD1X 
=  W1X 

=  DQ(8) 

=  IMTGRL(0. ,WD1Y) 

=  WD1Y 

=  W1Y 
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WD1Z  =  DQ(9) 

W1Z  =  INTGRL(0.,WD1Z) 

WDZ(l)  =  WD1Z 

Wl(3)  =  W1Z 

*  TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 

*  SYSTEM  FOR  THE  LINK  ONE 

MAT1R(1,1)  =  DCOS(RLRZl)  *  DCOS(PTRYl) 

MAT1R(2,1)  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DSIN(YWRXl)  -... 
DSIN(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3,1)  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  +... 
DSIN(RLRZl)  *  DSIN(YWRXl) 

MAT1R(1,2)  =  DSIN(RLRZl)  *  DCOS(PTRYl) 

MAT1R(2,2)  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DSIN(YWRXl)  +... 
DCOS(RLRZl)  *  DCOS(YWRXl) 

MAT1R(3,2)  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  -... 
DCOS(RLRZl)  *  DSIN(YWRXl) 

MAT1R(1,3)  =  -DSIN(PTRYl) 

MAT1R(2,3)  =  DCOS(PTRYl)  *  DSIN(YWRXl) 

MAT1R(3,3)  =  DCOS(PTRYl)  *  DCOS(YWRXl) 

*  GET  THE  VELOCITIES  FOR  LINK  1  IN  BODY  FIXED  COOR.  SYSTEM 

DO  605  J  =  1,3 
SUM1  =  0.0D0 

DO  606  K  =  1,3 

SUM1  =  SUM1  +  MAT1R(J,K)  *  W1(K) 
606         CONTINUE 

BRATEl(J)  =  SUM1 
605     CONTINUE 

*  TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  EULER  COORDINATE 

*  SYSTEM  FOR  THE  LINK  ONE 

MAT1T(1,1)  =  0.0D0 
MAT1T(2,1)  =  1.0D0 
MAT1T(3,1)    =   0.0D0 

MAT1T(1,2)  =   DCOS(YWRXl) 


MAT1T(2,2  =   DTAN(PTRYl)  *  DSIN(YWRXl) 

MAT1T(3,2)  =   1.0DO/DCOS(PTRY1)  *  DSIN(YWRXl) 

MAT1T(1,3)  =  -DSIN(YWRXl) 

MAT1T(2,3)  =   DTAN(PTRYl)  *  DCOS(YWRXl) 

MAT1T(3,3)  =   l.D0/DCOS(PTRYl)  *  DCOS(YWRXl) 

GET  THE  YAW, PITCH  AND  THE  ROLL  RATES  FOR  LINK  ONE 

DO  705  J  =  1,3 


SUM1  =  0.0D0 

DO  706  K  =  1,3 

SUM1  =  SUM1  +  MAT1T(J,K)  *  BRATEl(K) 

706 

CONTINUE 

RATEl(J)  =  SUM1 

705 

CONTINUE 

RATE1X  =  RATEl(l) 
RATE1Y  =  RATE1(2) 
RATE1Z  =  RATE1(3) 

YWRX1  =  INTGRL(0. ,RATE1X) 
PTRY1  =  INTGRL(0. ,RATE1Y) 
RLRZ1  =  INTGRL(0. ,RATE1Z) 

YAWANX(l)  =  YWRX1  *  RADEG 
PTCANY(l)  =  PTRY1  *  RADEG 
ROLANZ(l)  =  RLRZ1  *  RADEG 

DIRECTION  COSINES  FOR  THE  FIRST  LINK  SAME  FOR  EACH  CASE 

DRCSY(l)  =  DCOS(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  +. 
DSIN(RLRZl)  *  DSIN(YWRXl) 
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GET 


DRCSX(l)  =  DSIN(RLRZl)  *  DSIN(PTRYl)  *  DCOS(YWRXl)  -... 
DCOS(RLRZl)  *  DSIN(YWRXl) 

DRCSZ(l)  =  DCOS(PTRYl)  *  DCOS(YWRXl) 

THE  ANGLES  AS  RADIANS 


DRCRAX 
DRCRAY 
DRCRAZ 


DACOS' 
DACOS' 
DACOS i 


DRCSX 
DRCSY 
DRCSZ 


CONVERT  THE  DIRECTION  COSINES 


DRCANX(l) 
DRCANY(l) 
DRCANZ ( 1 ) 

LINK  TWO 

AX2 

VELX2 

LCOGX2 

LCOGX(2> 

AY2 
VELY2 
LCOGY2 
LCOGY(2) 

AZ2 
VELZ2 
LCOGZ2 
LCOGZ(2) 

WD2X 
W2X 

THE  INIT. 
KEEP  THE 

THE  NEXT 

THETRD 

WDX(2) 
W2(l) 

WD2Y 
W2Y 


=  DACOS' 
=  DACOS' 
=  DACOS' 


DRCSX 
DRCSY 
DRCSZ 


DEGREES 

RADEG 
RADEG 
RADEG 


USE 

AND 

USE 


DQ(13) 

=  INTGRL(0.,AX2) 
=  INTGRL(X2,VELX2) 
=  LCOGX2 

=  DQ(14) 

=  INTGRL(0.,AY2) 

=  INTGRL(Y2,VELY2) 

=  LCOGY2 

=  DQ(15) 

=  INTGRL(0. ,AZ2) 

=  INTGRL(Z2,VELZ2) 

=  LCOGZ2 

=  DQ(16) 

=  INTGRL((PI**2)/4.,WD2X) 

COND.  WITH  ONLY  WHICHEVER  VELOCITY  APPLIED 
TWO  OTHER  ANG.  VEL.  INIT.  COND.  AS  ZERO 

STATEMENT  IF  THE  ANGULAR  VELOCITY  IS  IN  THE  X  DIR. 

=  INTGRL(0. ,W2X) 

=  WD2X 
=  W2X 


USE  THE  NEXT 

THETRD 

WDY(2) 
W2(2) 

WD2Z 
W2Z 

USE  THE  NEXT 

THETRD    = 

WDZ(2) 
W2(3) 

THETAD  = 
ERROR 


=  DQ(17) 

=  INTGRL(0. ,WD2Y) 

STATEMENT  IF  THE  ANGULAR  VELOCITY  IS  IN  THE  Y  DIR. 

=  INTGRL(0.,W2Y) 

=  WD2Y 
=  W2Y 

=  DQ(18) 

=  INTGRL(0.,WD2Z) 


STATEMENT 

'  INTGRL(0, 

=  WD2Z 
=  W2Z 


IF  THE  ANGULAR  VELOCITY  IS  IN  THE  Z  DIR. 
,W2Z) 


THETRD  *  RADEG 
ABS(((THETAD-THETAB)/180.)  *  100) 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COORDINATE 
SYSTEM  FOR  THE  LINK  TWO 

MAT2R(1,1)  =  DCOS(RLRZ2)  * 

MAT2R(2,1)  =  DCOS(RLRZ2)  * 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

MAT2R(3,1)  =  DCOS(RLRZ2)  * 
DSIN(RLRZ2)  *  DSIN(YWRX2) 

MAT2R(1,2)  =  DSIN(RLRZ2)  * 

MAT2R(2,2)  =  DSIN(RLRZ2)  * 


DCOS(PTRY2) 

DSIN(PTRY2)  *  DSIN(YWRX2) 

DSIN(PTRY2)  *  DCOS(YWRX2) 


+  . 


DCOS(PTRY2) 
DSIN(PTRY2) 


*  DSIN(YWRX2)  +. 
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608 
607 


DCOS(RLRZ2)  *  DCOS(YWRX2) 

MAT2R(3,2)  =  DSIN(RLRZ2)  * 
DCOS(RLRZ2)  *  DSIN(YWRX2) 

MAT2R(1,3)  =  -DSIN(PTRY2) 

MAT2R(2,3)  =  DCOS(PTRY2)  * 

MAT2R(3,3)  =  DCOS(PTRY2)  * 

GET  THE  VELOCITIES  FOR  LINK  2 


DSIN(PTRY2)  *  DCOS(YWRX2) 

DSIN(YWRX2) 
DCOS(YWRX2) 
IN  BODY  FIXED  COOR.  SYSTEM 


DO  607  J  =  1,3 
SUM1  =  0.0D0 

DO  608  K  =  1,3 
SUM1  =  SUM1  + 
CONTINUE 

BRATE2(J)  =  SUM1 
CONTINUE 


MAT2R(J,K)  *  W2(K) 


TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  EULER  COOR.  SYSTEM 
FOR  THE  LINK  TWO 

MAT2T(1,1)  =  0.0D0 
MAT2T(2/1)  =  1.0D0 
MAT2T(3,1)  =  0.0D0 

MAT2T(1,2)  =   DCOS(YWRX2) 

MAT2T(2,2)  =   DTAN(PTRY2)  *  DSIN(YWRX2) 

MAT2T(3,2)  =   1 .0D0/DCOS(PTRY2)  *  DSIN(YWRX2) 

MAT2T(1,3)  =  -DSIN(YWRX2) 

MAT2T(2,3)  =   DTAN(PTRY2)  *  DCOS(YWRX2) 

MAT2T(3,3)  =   1 .0D0/DCOS (PTRY2)  *  DCOS(YWRX2) 

GET  THE  YAW, PITCH  AND  THE  ROLL  RATES  FOR  LINK  TWO 

DO  707  J  =  1,3 


SUM1  =  0.0D0 

DO  708  K  =  1,3 

SUM1  =  SUM1  + 

708 

CONTINUE 

RATE2(J)  =  SUM1 

707 

CONTINUE 

MAT2T(J,K)  *  BRATE2(K) 


RATE2X  =  RATE2(1) 
RATE2Y  =  RATE2(2) 
RATE2Z  =  RATE2(3) 

USE  THE  NEXT  THREE  STATEMENTS  FOR  CASE  A 

YWRX2  =  INTGRL(0. ,RATE2X) 
PTRY2  =  INTGRLfO. ,RATE2Y) 
RLRZ2  =  INTGRL(-PI/2. ,RATE2Z) 

USE  THE  NEXT  THREE  STATEMENTS  FOR  CASE  B 

YWRX2  =  INTGRL(0.,RATE2X; 
PTRY2  =  INTGRL(0. ,RATE2Y; 
RLRZ2  =  INTGRL(PI/2.,RATE2Z) 

USE  THE  NEXT  THREE  STATEMENTS  FOR  CASE  C 

YWRX2  =  INTGRL(0. ,RATE2X) 
PTRY2  =  INTGRLCO. ,RATE2Y) 
RLRZ2  =  INTGRL(0. ,RATE2Z) 

1)  =  RATE2X 
=  RATE 2 Y 
=  RATE2Z 


RATE  2 
RATE  2 
RATE  2 

YAWANXI 
PTC ANY i 
ROLANZ i 


=  YWRX2 
=  PTRY2 
=  RLRZ2 


RADEG 
RADEG 
RADEG 


USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  TWO  FOR  CASE  A 

DRCSY(2)  =  DCOS(RLRZ2)  *  DSIN(PTRY2)  *  DSIN(YWRX2)  -... 
DSIN(RLRZ2)  *  DCOS(YWRX2) 

DRCSX(2)  =  DSIN(RLRZ2)  *  DSIN(-PTRY2)  *  DSIN(YWRX2)  +... 
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* 

* 

* 
* 
* 


* 
* 


6 


DC0S(RLRZ2)  *  DC0S(YWRX2) 
DRCSZ(2)  =  DC0S(PTRY2)  *  DSIN(YWRX2) 
USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  TWO  FOR  CASE  B 

DRCSY(2)  =  DCOS(RLRZ2)  *  DCOS(PTRY2) 
DRCSX(2)  =  DSIN(RLRZ2)*DCOS(PTRY2) 

DRCSZ(2)  =  -DSIN(PTRY2) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  TWO  FOR  CASE  C 

DRCSY(2)  =  DCOS(RLRZ2)  *  DSIN(PTRY2)  *  DCOS(YWRX2)  +... 
DSIN(RLRZ2)  *  DSIN(YWRX2) 

DRCSX(2)  =  DSIN(RLRZ2)  *  DSIN(PTRY2)  *  DCOS(YWRX2)  -... 
DCOS(RLRZ2)  *  DSIN(YWRX2) 

DRCSZ(2)  =  DCOS(PTRY2)  *  DCOS(YWRX2) 

GET  THE  ANGLES  AS  RADIANS 

DRCRAX(2)  =  DACOS(DRCSX(2)) 
DRCRAY(2)  =  DACOS(DRCSY(2)) 
DRCRAZ(2)  =  DACOS(DRCSZ(2)) 

CONVERT  THE  DIRECTION  COSINES  TO  DEGREES 

DRCANX(2)  =  DACOS(DRCSX(2))  *  RADEG 
DRCANY(2)  =  DACOS(DRCSY(2))  *  RADEG 
DRCANZ(2)  =  DACOS(DRCSZ(2))  *  RADEG 

FIND  THE  JOINT  LOCATION 


DCOS(DRCRAX(l' 

DCOS(DRCRAY(l 

DCOS(DRCRAZ(l 


=  DQ(22) 

=  INTGRL(0. ,AX3) 

=  INTGRL(X3,VELX3) 

=  LCOGX3 

=  DQ(23) 

=  INTGRL(0. ,AY3) 

=  IMTGRL(Y3,VELY3) 

=  LCOGY3 


=  DQ(24) 

=  INT" 


JX1  = 
JY1  = 
JZ1   = 

LINK  THREE 

AX3 
VELX3 
LCOGX3 
LCOGX(3) 

AY3 
VELY3 
LCOGY3 
LCOGY(3) 

AZ3 
VELZ3 
LCOGZ3 
LCOGZ(3) 

WD3X 
W3X 
WDX(3) 
W3(l) 

WD3Y 
W3Y 
WDY(3) 
W3(2) 

WD3Z 
W3Z 
WDZ(3) 
W3(3) 

TRANSFORMATION  MATRIX  FROM  EARTH  FIXED  TO  BODY  FIXED  COOR. 
FOR  THE  LINK  THREE 

MAT3R(1,1)  =  DCOS(RLRZ3)  * 

MAT3R(2,1)  =  DCOS(RLRZ3)  * 
DSIN(RLRZ3)  *  DCOS(YWRX3) 

MAT3R(3,1)  =  DCOS(RLRZ3)  * 


JTGRL(0.,AZ3) 
=  INTGRL(Z3,VELZ3) 
=  LCOGZ3 

=  DQ(25) 

=  INTGRL(0.,WD3X) 

=  WD3X 

=  W3X 

=  DQ(26) 

=  INTGRL(0.,WD3Y) 

=  WD3Y 

=  W3Y 

=  DQ(27) 

=  INTGRL(0. ,WD3Z) 

=  WD3Z 

=  W3Z 


SYSTEM 


DCOS(PTRY3) 
DSIN(PTRY3) 

DSIN(PTRY3) 


DSIN(YWRX3)  -... 
DCOS(YWRX3)  +. .. 
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DSIN(RLRZ3)  *  DSIN(YWRX3) 

MAT3R(1,2)  =  DSIN(RLRZ3)  * 

MAT3R(2,2)  =  DSIN(RLRZ3)  * 
DC0S(RLRZ3)  *  DC0S(YWRX3) 

MAT3R(3,2)  =  DSIN(RLRZ3)  * 
DC0S(RLRZ3)  *  DSIN(YWRX3) 

MAT3R(1,3)  =  -DSIN(PTRY3) 

MAT3R(2,3)  =  DC0S(PTRY3)  * 

MAT3R(3,3)  =  DC0S(PTRY3)  * 

GET  THE  VELOCITIES  FOR  LINK  3 


DCOS(PTRY3) 
DSIN(PTRY3) 

DSIN(PTRY3) 


DSIN(YWRX3) 

DCOS(YWRX3) 

IN  BODY  FIXED 


DSIN(YWRX3)  +.. . 
DCOS(YWRX3) 


COOR.  SYSTEM 


DO  609  J  =  1,3 
SUM1  =  0.0D0 

DO  610  K  =  1,3 

SUM1  =  SUM1  +  MAT3R(J,K)  *  W3(K) 
610         CONTINUE 

BRATE3(J)  =  SUM1 
609     CONTINUE 

TRANSFORMATION  MATRIX  FROM  BODY  FIXED  TO  EULER  COOR. 
FOR  THE  LINK  THREE 

MAT3T(1,1)  =  0.0D0 
MAT3T(2,1)  =  1.0D0 
MAT3T(3,1)    =   0.0D0 

=   DCOS(YWRX3) 

=   DTAN(PTRY3)  *  DSIN(YWRX3) 

=   1.0D0/DCOS(PTRY3)  *  DSIN(YWRX3) 

=  -DSIN(YWRX3) 

=   DTAN(PTRY3)  *  DCOS(YWRX3) 

=   1.0D0/DCOS(PTRY3)  *  DCOS(YWRX3) 

GET  THE  YAW, PITCH  AND  THE  ROLL  RATES  FOR  LINK  THREE 


SYSTEM 


MAT3T 
MAT3T 
MAT3T 

MAT3T 
MAT3T 
MAT3T 


DO  709  J  =  1,3 

SUM1  =  0.0D0 

DO  710  K  =  1,3 

SUM1  =  SUM1  +  MAT3T(J,K)  *  BRATE3(K) 

710 

CONTINUE 

RATE3(J)  =  SUM1 

709 

CONTINUE 

RATE3X  =  RATE3< 
RATE3Y  =  RATE3( 
RATE3Z  =  RATE3I 

USE  THE  NEXT  THREE 


YWRX3  = 
PTRY3  = 
RLRZ3  = 


INTGRL 
INTGRL 
INTGRL 


FOR  THE  CASE  A 

0. ,RATE3X) 
0. ,RATE3Y) 
-PI/2. ,RATE3Z) 

FOR  THE  CASE  B 


USE  THE  NEXT  THREE 

YWRX3  =  INTGRL ( 0. ,RATE3X) 
PTRY3  =  INTGRL ( 0. ,RATE3Y) 
RLRZ3  =  INTGRL(PI/2. ,RATE3Z) 

USE  THE  NEXT  THREE  FOR  THE  CASE  C 

YWRX3  =  INTGRL ( 0. , RATE 3X) 
PTRY3  =  INTGRL ( 0. ,RATE3Y) 
RLRZ3  =  INTGRL ( 0. ,RATE3Z) 

YAWANX(3)  =  YWRX3  *  RADEG 
PTCANY(3)  =  PTRY3  *  RADEG 
ROLANZ(3)  =  RLRZ3  *  RADEG 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  THREE  FOR  CASE  A 

DRCSY(3)  =  DCOS(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)  -... 
DSIN(RLRZ3)  *  DCOS(YWRX3) 
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DRCSX(3)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  *  DSIN(YWRX3)+. . . 
DC0S(RLRZ3)  *  DC0S(YWRX3) 

DRCSZ(3)  =  DC0S(PTRY3)  *  DSIN(YWRX3) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  THREE  FOR  CASE  B 
DRCSY(3)  =  DCOS(RLRZ3)  *  DCOS(PTRY3) 
DRCSX(3)  =  DSIN(RLRZ3)*DCOS(PTRY3) 

DRCSZ(3)  =  -DSIN(PTRY3) 

USE  THE  NEXT  SET  OF  THE  DIRECTION  COSINES  FOR  LINK  THREE  FOR  CASE  C 

DRCSY(3)  =  DCOS(RLRZ3)  *  DSIN(PTRY3)  * 
DSIN(RLRZ3)  *  DSIN(YWRX3) 

DRCSX(3)  =  DSIN(RLRZ3)  *  DSIN(PTRY3)  * 
DCOS(RLRZ3)  *  DSIN(YWRX3) 

DRCSZ(3)  =  DCOS(PTRY3)  *  DCOS(YWRX3) 

GET  THE  ANGLES  AS  RADIANS 

DRCRAX(3)  =  DACOS(DRCSX(3)) 
DRCRAY(3)  =  DACOS(DRCSY{3)) 
DRCRAZ(3)  =  DACOS(DRCSZ(3)J 

CONVERT  THE  DIRECTION  COSINES  TO  DEGREES 


DCOS(YWRX3)+.. . 
DCOS(YWRX3)-... 


DRCANX(3)  =  DACOS(DRCSX(3 
DRCANY(3)  =  DACOS(DRCSY(3' 
DRCANZ(3)  =  DACOS(DRCSZ(3' 


*  RADEG 

*  RADEG 

*  RADEG 


FIND  ANGLE  BETWEEN  LINK  2  AND  LINK  3  TO  CHECK  IF  THE  ARM  LINKS 
are  PASSING  THROUGH  THE  SINGULAR  POINTS 


ANG23X 
ANG23Y 
ANG23Z 

ANG12X 
ANG12Y 
ANG12Z 


DRCANXI 
DRCANYI 
DRCANZI 

DRCANXI 
DRCANYI 
DRCANZI 


DRCANXI 
DRCANYI 
DRCANZI 

DRCANXI 
DRCANYI 
DRCANZI 


FIND  THE  JOINT  LOCATION 

JX2  =  JX1  + 
JY2  =  JY1  + 
JZ2  =  JZ1  + 

TIPX  =  JX2  + 
TIPY  =  JY2  + 
TIPZ  =  JZ2  + 


DCOS(DRCRAXi 
DCOS(DRCRAYl 
DCOS(DRCRAZi 

*  DCOS(DRCRAX(3 

*  DCOS(DRCRAY(3' 

*  DCOS(DRCRAZ(3' 


END 

STOP 

FORTRAN 


SUBROUTINE  TO  COMPUTE  THE  CROSS  PRODUCT  OF  TWO  VECTORS 

SUBROUTINE  CPROD(VECTA, VECTB ,MI ,MJ,MK) 
IMPLICIT  REAL*8  (A-Z) 
DIMENSION  VECTA(3) , VECTB (3) 
MI  =  VECTA(2)  *  VECTB(3)  -  VECTA 

*  VECTB (1 

*  VECTB (2 


MJ  =  VECTA (3 
MK  =  VECTA(1' 

RETURN 

END 


-  VECTA 

-  VECTA 


VECTB i 
VECTB i 
VECTB i 
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